/*
 *  Brick.cpp
 *  EPPI-0.0
 *
 *  Created by Juan Carlos Garcia Orden on 15/11/07.
 *	Modified by Sergio Conde 16/09/2010 and  04/2011
 *
 */

#include "brick.h"

Brick::Brick()
:	Component(),
	material(),
	integShape()
{}

// ***

Brick::Brick(vector<string>& vp, string& mn, int di)
:	Component(vp,mn,di),
	material(),
	partial_qddotOld(0.0),
	detJ0(1.0),
	alpha(0.0),
	DeltaCDeltaC(3.0),
	SMidDeltaC(0.0),
	stEner(0.0),
	integShape()
{
	
	gau.setZero(8,3);        // Gauss points matrix    (xi, eta, zeta) 8x3
	lob.setZero(27,3);        // Lobatto points matrix    (xi, eta, zeta) 27x3
	H0.setZero(6,9);         // Matrix for Bnl calculation 6x9
	
	Q.setZero(8,3);          // Coordinates (deformed) 8x3
	Qref.setZero(8,3);       // Coordinates (reference) 8x3
	
	H.setZero(3,8);  
	vG.setZero(24,8);	
	G.setZero(9,24);      // 9 x 24
	U.setZero(24);        // Displacement vector 24x1
	the.setZero(9);     // Vector of displacements derivatives 9x1
	A.setZero(6,9);       // Matrix for Bnl calculation 6x9
	B.setZero(6,24);      // Matrix Bnl (total) 6x24
	
	C.setZero(6);         // Right Cauchy-Green Deformation Tensor
	C << 1.0,
	     1.0,
	     1.0,
	     0.0,
	     0.0,
	     0.0;
	
	S.setZero(6);       // Second Piola-Kirchhoff stress tensor (Vector format)
	
	H0(0,0) = 1.;
	H0(1,4) = 1.;
	H0(2,8) = 1.;
	H0(3,1) = 1.;
	H0(3,3) = 1.;
	H0(4,2) = 1.;
	H0(4,6) = 1.;
	H0(5,5) = 1.;
	H0(5,7) = 1.;
	
	
	gau(0,0) = -0.5773502693;
	gau(0,1) = -0.5773502693;
	gau(0,2) = -0.5773502693;
	gau(1,0) =  0.5773502693;
	gau(1,1) = -0.5773502693;
	gau(1,2) = -0.5773502693;
	gau(2,0) =  0.5773502693;
	gau(2,1) =  0.5773502693;
	gau(2,2) = -0.5773502693;
	gau(3,0) = -0.5773502693;
	gau(3,1) =  0.5773502693;
	gau(3,2) = -0.5773502693;
	gau(4,0) = -0.5773502693;
	gau(4,1) = -0.5773502693;
	gau(4,2) =  0.5773502693;
	gau(5,0) =  0.5773502693;
	gau(5,1) = -0.5773502693;
	gau(5,2) =  0.5773502693;
	gau(6,0) =  0.5773502693;
	gau(6,1) =  0.5773502693;
	gau(6,2) =  0.5773502693;
	gau(7,0) = -0.5773502693;
	gau(7,1) =  0.5773502693;
	gau(7,2) =  0.5773502693;

	lob(0,0) = -1.0;
	lob(0,1) = -1.0;
	lob(0,2) = -1.0;
	lob(1,0) =  1.0;
	lob(1,1) = -1.0;
	lob(1,2) = -1.0;
	lob(2,0) =  1.0;
	lob(2,1) =  1.0;
	lob(2,2) = -1.0;
	lob(3,0) = -1.0;
	lob(3,1) =  1.0;
	lob(3,2) = -1.0;
	lob(4,0) = -1.0;
	lob(4,1) = -1.0;
	lob(4,2) =  1.0;
	lob(5,0) =  1.0;
	lob(5,1) = -1.0;
	lob(5,2) =  1.0;
	lob(6,0) =  1.0;
	lob(6,1) =  1.0;
	lob(6,2) =  1.0;
	lob(7,0) = -1.0;
	lob(7,1) =  1.0;
	lob(7,2) =  1.0;

//	lob(0,0) = -1.0;
//	lob(0,1) = -1.0;
//	lob(0,2) = -1.0;
//	lob(1,0) =  0.0;
//	lob(1,1) = -1.0;
//	lob(1,2) = -1.0;
//	lob(2,0) =  1.0;
//	lob(2,1) = -1.0;
//	lob(2,2) = -1.0;
//	lob(3,0) =  1.0;
//	lob(3,1) =  0.0;
//	lob(3,2) = -1.0;
//	lob(4,0) =  1.0;
//	lob(4,1) =  1.0;
//	lob(4,2) = -1.0;
//	lob(5,0) =  0.0;
//	lob(5,1) =  1.0;
//	lob(5,2) = -1.0;
//	lob(6,0) = -1.0;
//	lob(6,1) =  1.0;
//	lob(6,2) = -1.0;
//	lob(7,0) = -1.0;
//	lob(7,1) =  0.0;
//	lob(7,2) = -1.0;
//	lob(8,0) = -1.0;
//	lob(8,1) = -1.0;
//	lob(8,2) = -1.0;
//
//	lob(9,0) = -1.0;
//	lob(9,1) = -1.0;
//	lob(9,2) =  0.0;
//	lob(10,0) =  0.0;
//	lob(10,1) = -1.0;
//	lob(10,2) =  0.0;
//	lob(11,0) =  1.0;
//	lob(11,1) = -1.0;
//	lob(11,2) =  0.0;
//	lob(12,0) =  1.0;
//	lob(12,1) =  0.0;
//	lob(12,2) =  0.0;
//	lob(13,0) =  1.0;
//	lob(13,1) =  1.0;
//	lob(13,2) =  0.0;
//	lob(14,0) =  0.0;
//	lob(14,1) =  1.0;
//	lob(14,2) =  0.0;
//	lob(15,0) = -1.0;
//	lob(15,1) =  1.0;
//	lob(15,2) =  0.0;
//	lob(16,0) = -1.0;
//	lob(16,1) =  0.0;
//	lob(16,2) =  0.0;
//	lob(17,0) = -1.0;
//	lob(17,1) = -1.0;
//	lob(17,2) =  0.0;
//
//	lob(18,0) = -1.0;
//	lob(18,1) = -1.0;
//	lob(18,2) =  1.0;
//	lob(19,0) =  0.0;
//	lob(19,1) = -1.0;
//	lob(19,2) =  1.0;
//	lob(20,0) =  1.0;
//	lob(20,1) = -1.0;
//	lob(20,2) =  1.0;
//	lob(21,0) =  1.0;
//	lob(21,1) =  0.0;
//	lob(21,2) =  1.0;
//	lob(22,0) =  1.0;
//	lob(22,1) =  1.0;
//	lob(22,2) =  1.0;
//	lob(23,0) =  0.0;
//	lob(23,1) =  1.0;
//	lob(23,2) =  1.0;
//	lob(24,0) = -1.0;
//	lob(24,1) =  1.0;
//	lob(24,2) =  1.0;
//	lob(25,0) = -1.0;
//	lob(25,1) =  0.0;
//	lob(25,2) =  1.0;
//	lob(26,0) = -1.0;
//	lob(26,1) = -1.0;
//	lob(26,2) =  1.0;
}

// ***

void Brick::calcFi(const vector<double>& qddot)
{
	int i;
	int j;

	Fi.setZero(numDofs);
	// Mechanical contribution to Fi = M * qddot
	for(i=0; i<numDofsInertia; ++i)
	{
		if (M.size()!=0)	// Consistent mass matrix
		{
			for(j=0; j<numDofsInertia; ++j)
			{
				if(integratorType==0)	Fi[i] += M(i,j) * qddot[ localGlob[j] ];
				else	Fi[i] += M(i,j) * ( dq[j] - dqC[j] );
			}
		}
		else	// Lumped mass matrix
		{
			if(integratorType==0)	Fi[i] += MLumped[i] * qddot[ localGlob[i] ];
			else Fi[i] += MLumped[i] * ( dq[i] - dqC[i] );
		}
	}
}

// ***

void Brick::calcFq(const int& withK)
{
	// withK=0: Simultaneous exact tangent computation
	// withK=1: No tangent calculation
	
	int i, j, k;
	
	Fq.setZero(numDofs);
	
	if(withK == 0)
	{
		Kq.setZero(numDofs,numDofs);
	  
		if(!material->type.compare("viscoelastic") and delta == 0.0)
		{
             if( viscoMode == 0 )
                 for(i=0;i<nInt;++i) delta += vEta[i] / vTau[i];
			 else
                 for(i=0;i<nInt;++i) delta += vBeta[i] * exp(- deltaT / (2.0 * vTau[i]) );
		}
	}
	
	setQfromq();
	makeU();
	
	stEner = 0.0;
    dissEner = 0.0;
	
    if(integratorType == 0 or integratorType == 1) // Means standard integrator
    {
		for (k=0; k<8; ++k)	// Loop on Gauss points
		{
    		double xi, eta, zeta;

    		xi = gau(k,0);
            eta = gau(k,1);
            zeta = gau(k,2);

			makeG(k);
            makeTheta();
            makeA();
            makeB();
            makeJ0(xi, eta, zeta);	// Computation of H, J0 and J0i
            makeF();
            makeC();

            //cout << "F("<< k+1 <<")= "<< endl << F << endl;

            if(material->type.compare("viscoelastic") and material->type.compare("thermoelastic") and material->type.compare("thermoviscoelastic") and material->type.compare("damage"))
            {
            	// Hyperelasticity
                material->calcS(C);
                S = material->getS();

                material->calcW(C);
                stEner += material->getW() * detJ0;
            }
            else if (!material->type.compare("damage"))
            {
            	// Hyperelasticity with damage
            	kappa = 0.0;
                material->calcS(C, kappa);
                S = material->getS();

                material->calcW(C, kappa);
                stEner += material->getW() * detJ0;
            }
            else if (!material->type.compare("viscoelastic"))
            {
            	// Viscoelasticity
            	if(!viscoMode) //Means internal variables pre-integrated.
            	{
            		material->calcS(C);
            		S = material->getS();

            		for(i=0; i<nInt ; ++i)
            		{
            			calcHv(i,k);	// Calculate History tensor
            			calcQv(i,k);	// Calculate Non-Equilibrium Stresses
            			S += Qv.segment<6>(6*i);
            		}

            		material->calcW(C);
            		stEner += material->getW() * detJ0;

            		for(i=0;i<nInt;++i)
            			dissEner += dotVoigt(Qv.segment<6>(6*i), Gam.segment<6>(6*i) - vGamConv.block(6*i,k,6,1)) * detJ0;
            	}
                else //Means internal variables to be integrated
                {
                	integrateInternalStrains(k,0);	 // From this point the internal variables (Gam) are computed.

                	material->calcS(C, Gam);
                	S = material->getS();

                	material->calcW(C, Gam);
                    stEner += material->getW() * detJ0;

                    for(i=0;i<nInt;++i)
                    {
                    	material->calcQv(i, C, Gam);
                    	dissEner += dotVoigt(material->getQv(), Gam.segment<6>(6*i) - vGamConv.block(6*i,k,6,1)) * detJ0;
                    }
                }
            }
            else if (!material->type.compare("thermoelastic"))
            {
            	//Thermoelasticity

            	//Evaluating temperature at Gauss point
            	Theta = Nt.row(k) * thetaVec;
        		cout << "Theta(" << k+1 << ")= " << Theta << endl;

        		material->calcS(C,Theta);
        		S = material->getS();

        		material->calcW(C,Theta);
        		stEner += material->getW() * detJ0;

        		material->calcEntropy(C,Theta);
        		Entropy += material->getEntropy();

            	if(thermoMode) // State variable: Temperature
            	{
            		// Gauss point contribution to Fq = Mt * intDotEnt * thetaVec
            		// Gauss point contribution to thermal power Pint = GradN^T * K*  C^{-1} * GradN * ThetaVec
            		// GradN = J0i * H is 3x8
            		// K is 3 x 3
            		// ThetaVec is 8x1
            		// Contribution is 8x1

            		double intDotEnt = 0.0;

                	if(integratorType == 0)
                	{
                		intDotEnt += (2.0 / deltaT * (material->getEntropy() - vEntConv(k)) - vdEntConv(k)) * detJ0 ;
                	}
                	else intDotEnt += ( material->getEntropy() - vEntConv(k) ) /  deltaT * detJ0 ;

            		material->calcConductivity(Theta);
            		Kond = H.transpose() * J0i.transpose() * material->getK() * CMat.inverse() * J0i * H * detJ0;

            		Fq.segment(numDofsInertia, 8) += (intDotEnt * Mt + Kond) * thetaVec;
            	}
            	else
            	{

            	}
            }
			
            // Gauss point contribution to Fq = B^T * S * detJ0
            // B^T is 24x6
            // S is 6x1
            // Contribution is 24x1
			
            Fq.segment(0, numDofsInertia) += B.transpose() * S * detJ0;

            if(withK == 0)
            {
            	makeKq(k);	// Simultaneous calculation of Kq
            }
        }
    }
	else if (integratorType == 2 )// Means Consistent integrator
    {
		for (k=0; k<8; ++k)     // Loop on Gauss points
	    {
    		double xi, eta, zeta;

    		xi = gau(k,0);
            eta = gau(k,1);
            zeta = gau(k,2);

			makeG(k);
			makeTheta();
            makeA();
            makeB();
            makeJ0(xi, eta, zeta);   // Computation of H, J0 and J0i
            makeF();
            makeC();

            CMid = 0.5 * (C + vCConv.col(k));
                 
            if (!material->type.compare("svk"))
            {
            	alpha = 0.0;
              	material->calcS(CMid);
                S = material->getS();
            }
            else if (material->type.compare("viscoelastic") and material->type.compare("thermoelastic") and material->type.compare("thermoviscoelastic") and material->type.compare("damage") and material->type.compare("svk"))
            {
            	// Hyperelasticity
            	material->calcS(CMid);
            	SMid = material->getS();
                  
                SMidDeltaC = dotVoigt(SMid, C - vCConv.col(k));
                DeltaCDeltaC = dotVoigt(C - vCConv.col(k),C - vCConv.col(k));
                  
                if(DeltaCDeltaC != 0.0)
                {
                	material->calcW(C);
                    alpha = (2.0 * (material->getW() - vWConv(k)) - SMidDeltaC) / DeltaCDeltaC;
                }
                else alpha = 0.0;

                S = SMid + alpha * (C - vCConv.col(k));

            	material->calcW(C);
            	stEner += material->getW() * detJ0;
            }
            else if (!material->type.compare("damage"))
            {
            	// Hyperelasticity with damage
            	cout << "To be done" << endl;
            }
            else if (!material->type.compare("viscoelastic"))
            {
            	// Viscoelasticity
            	 integrateInternalStrains(k,0);  // From this point the internal variables (Gam) are computed.

            	 material->calcS(CMid, Gam);
            	 SMid = 0.5 * material->getS();

            	 material->calcS(CMid,vGamConv.col(k));
            	 SMid += 0.5 * material->getS();

            	 SMidDeltaC = dotVoigt(SMid,C - vCConv.col(k));
            	 DeltaCDeltaC = dotVoigt(C - vCConv.col(k),C - vCConv.col(k));

            	 if(DeltaCDeltaC != 0.0)
            	 {
            		 material->calcW(C, Gam);
            		 alpha = material->getW();

            		 material->calcW(vCConv.col(k),Gam);
            		 alpha -= material->getW();

            		 material->calcW(C,vGamConv.col(k));
            		 alpha += material->getW();

            	     alpha += -vWConv(k) - SMidDeltaC;
            	     alpha /= DeltaCDeltaC;
            	 }
            	 else alpha = 0.0;

            	 S = SMid + alpha * (C - vCConv.col(k));

            	 material->calcW(C,Gam);
            	 stEner += material->getW() * detJ0;

            	 for(i=0;i<nInt;++i)
            	 {
            		 dissEner += dotVoigt(Qv.segment<6>(6*i) * vEta[i], Qv.segment<6>(6*i)) * deltaT * detJ0;
            		 //dissEner += dotVoigt(Qv.segment<6>(6*i), Gam.segment<6>(6*i) - vGamConv.block(6*k,i,6,nInt)) * vEta[i] * detJ0;
           	      	 //dissEner += dotVoigt(material->getQv(i,CMid,GamMid), Gam.segment<6>(6*i) - vGamConv.block(6*k,i,6,nInt)) * detJ0;
            	 }
            }
            else if (!material->type.compare("thermoelastic"))
            {
            	//Thermoelasticity

        		//Evaluating temperature/entropy at gauss point
        		Theta = Nt.row(k) * thetaVec;

            	material->calcS(C,Theta);
                S = material->getS();

                CMid = 0.5 * (C + vCConv.col(k));

				material->calcS(CMid, Theta);
				SMid = 0.5 * material->getS();
				material->calcS(CMid,vThetaConv(k));
				SMid += 0.5 * material->getS();

				SMidDeltaC = dotVoigt(SMid,C - vCConv.col(k));
				DeltaCDeltaC = dotVoigt(C - vCConv.col(k),C - vCConv.col(k));

				if(DeltaCDeltaC != 0.0)
				{
					material->calcW(C, Theta);
					alpha = material->getW();

					material->calcW(vCConv.col(k),Theta);
					alpha -= material->getW();

					material->calcW(C,vThetaConv(k));
					alpha += material->getW();

					alpha += -vWConv(k) - SMidDeltaC;
					alpha /= DeltaCDeltaC;
				}

				material->calcW(C,Theta);
				stEner += material->getW() * detJ0;

				if(thermoMode)	// State variable: Temperature
                {
                	// Gauss point contribution to Energy equation

                	Fq.segment(numDofsInertia, 8) += Nt * detJ0;
                }
                else	// State variable : Entropy
                {
                	Fq.segment(numDofsInertia, 8) += Nt * detJ0;
                }
            }

        	// Gauss point contribution to Fq = B^T * S * detJ0
        	// B^T is 24x6
        	// S is 6x1
        	// Contribution is 24x1

        	Fq.segment(0,numDofsInertia) += 0.5 * (B + vBConv.block(6*k,0,6,24)).transpose() * S * detJ0;

            if(withK == 0)
            {
            	makeKq(k);      // Simultaneous calculation of Kq
            }

	    }
      }
	  else if (integratorType == 3) // Means viscoelastic consistent integration for Internal Variables in Static analisys
      {
	        for (k=0; k<8; ++k)	// Loop on Gauss points
	        {
	    		double xi, eta, zeta;

	    		xi = gau(k,0);
	            eta = gau(k,1);
	            zeta = gau(k,2);

		        makeG(k);
		        makeTheta();
		        makeA();
		        makeB();
			    makeJ0(xi, eta, zeta);	// Computation of H, J0 and J0i
		        makeF();
		        makeC();
		        integrateInternalStrains(k,0);
			
		        material->calcS(C,Gam);
		        S = material->getS();
	        		
		       	// Gauss point contribution to Fq = B^T * S * detJ0
			    // B^T is 24x6
			    // S is 6x1
			    // Contribution is 24x1
			
			    Fq += B.transpose() * S * detJ0;
        
			    if(withK == 0)
			    {
			    	makeKq(k);	// Simultaneous calculation of Kq
			    }
			
			    //	cout << "kq_= " << endl << Kq << endl;
			    material->calcW(C,Gam);
			    stEner += material->getW() * detJ0;
                                
                for(i=0;i<nInt;++i)
                       dissEner += dotVoigt(Qv.segment<6>(6*i) * vEta[i], Qv.segment<6>(6*i)) * deltaT * detJ0;
	        }
      }
}

// ***

void Brick::calcFt(const double& t, const double& grav, const int& gravDir)
{}

// ***

void Brick::calcKi(double & partial_qddot)
{
	if(partial_qddot != partial_qddotOld)	// In order to compute Ki just once
	{
//		int i;
//		int j;
//		for(i=0; i<numDofs; i++)
			if (MLumped.size()==0)	// Consistent mass matrix
			{
				//for(j=0; j<numDofs; j++) Ki[i][j] = M(i,j)*partial_qddot;
				Ki.block(0,0,numDofsInertia, numDofsInertia) = M * partial_qddot;
			}
			else	// Lumped mass matrix
			{
				//Ki[i][i] = MLumped[i]*partial_qddot;
				for(int i=0; i<numDofs; i++)
					Ki(i,i) = MLumped[i]*partial_qddot;

			}

	}
	partial_qddotOld = partial_qddot;
}

// ***

void Brick::calcKq(int withK)
{
//	if (integratorType==2)	calcLocalKqNumeric();
}	

// ***
void Brick::calcKdq(double & partial_qdot)
{}

// ***

void Brick::calcM(const vector<double>& q, const double& t)
{
	int i;
	int j;
	int k;
	
	if(M.size()==0)	// Just in case M has not been created in initializeCompRes
	{
		M.setZero(numDofsInertia, numDofsInertia);
	}

	if(N.size()==0)
	{
		N.setZero(3,numDofsInertia);
	}

	for (k=0; k<8; ++k)	// Loop on Gauss points
	{	
		double xi = gau(k,0);
		double eta = gau(k,1);
		double zeta = gau(k,2);

		makeJ0(xi, eta, zeta);	// Computation of H, J0 and J0i
		
		for (i=0; i<8; ++i)	// Loop on shape function
		{
			N.block(0,3*i,3,3) = calcShape(i, xi, eta, zeta) * Matrix3d::Identity(3, 3) ;

		}

		M += material->getDensity() * N.transpose() * N * detJ0;
	}

	//cout << "M= " << endl << M << endl;

	if(!material->type.compare("thermoelastic") or !material->type.compare("thermoviscoelastic"))
	{
		if( Nt.size()==0 )	Nt.setZero(8,8);
		if( Mt.size()==0 )	Mt.setZero(8,8);

		for (k=0; k<8; ++k)	// Loop on Gauss points
		{
			double xi = gau(k,0);
			double eta = gau(k,1);
			double zeta = gau(k,2);

			makeJ0(xi, eta, zeta);	// Computation of H, J0 and J0i

			for (i=0; i<8; ++i)	// Loop on shape function
			{
				Nt(k,i) = calcShape(i, xi, eta, zeta);
			}

			Mt += Nt.row(k).transpose() * Nt.row(k) * detJ0;
		}
	}

	//cout << "Mt= " << endl << Mt << endl;
//	M.setZero(numDofsInertia, numDofsInertia);
//
//	for (k=0; k<8; ++k)	// Loop on Lobatto points
//	{
//		double xi = lob(k,0);
//		double eta = lob(k,1);
//		double zeta = lob(k,2);
//
//		makeJ0(xi, eta, zeta);	// Computation of H, J0 and J0i
//
//		for (i=0; i<8; ++i)	// Loop on shape function
//		{
//			N.block(0,3*i,3,3) = calcShape(i, xi, eta, zeta) * Matrix3d::Identity(3, 3) ;
//		}
//
//		M +=  material->getDensity() * N.transpose() * N * detJ0;
//	}
}

// ***
void Brick::calcMLumped(const vector<double>& q, const double& t)
{
	// Diagonal terms of the consistent matrix, scaled to account for the total mass
	int i;
	if(M.size()==0)
	{	
		M.setZero(numDofsInertia,numDofsInertia);
		calcM(q,t);	// Mass matrix can be defined once for all, since it is constant
	}
	if (MLumped.size()==0)
	{
		MLumped.resize(numDofsInertia);
	}
	
	// Scaling
	double sumDiag = 0.0;
	for (i=0; i<M.cols(); ++i)	sumDiag += M(i,i);
	
	for (i=0; i<numDofsInertia; ++i)	MLumped[i] = M(i,i) * calcElementMass() * dim/sumDiag;
}

// ***

void Brick::setDofs(void)
{
	int DofsTemp = 0;
	map<string,Point*,less<string> >::iterator i;

	if(!material->type.compare("thermoelastic") or !material->type.compare("thermoviscoelastic")) DofsTemp = 1;

	for(i=mPoints.begin(); i!=mPoints.end(); i++)	/** Loop on points belonging to this component */
	{
		for(int j=0; j<dim+DofsTemp; j++)	(*i).second->setDof(0,j);	// Only translational dof */
		for(int j=0; j<dim; j++)	(*i).second->setDofInertia(0,j);
	}

	numDofs = (dim + DofsTemp) * pointNames.size();
	numDofsInertia = dim * pointNames.size();
}

// ***

void Brick::initializeGlobalDofs(vector<double> &q_in, vector<double> &dq_in, const double & t)
{
//	map<string,Point*,less<string> >::iterator it;
	int i;
	int j;
	int p;
	
	// Initial position
	q.resize(numDofs);
	i=0;
	for(p=0; p<pointNames.size(); ++p)
//		for(it=mPoints.begin(); it!=mPoints.end(); ++it)
	{
		for(j=0; j<dim; j++)
		{
			// Default: initial position dofs are point coordinates
			// they go to the firsts positions
//				q_in( localGlob(i*numDofs+j) ) = it->second->r(j) ;
			q_in[ localGlob[i*dim+j] ] = mPoints[pointNames[p]]->r[j] ;
		}
		i++;
	}

	// Initial velocity
	// Default: initial velocity dofs are null
	dq.resize(numDofs);
	int l;
	int m;
	for (l=0; l < 8 ; ++l)
	{
		for (m=0; m < 3 ; ++m)
		{
			Qref(l,m) = q_in[ localGlob[3*l+m] ];
		}
	}
	
	makevG();

	if(!material->type.compare("viscoelastic"))
	{
		nInt = material->getNIntVars();
		viscoMode = material->getMode();
		Gam.setZero(6*nInt);
                
		if (!viscoMode) // I.vars pre-integrated: viscoMode == false
		{
			vTau.setZero(nInt);
			vBeta.setZero(nInt);
			vTau = material->getTau();
			vBeta = material->getBeta();
		}
		else // I.Vars to be integrated : viscoMode == true
		{
			vEta.setZero(nInt);
			vTau.setZero(nInt);
            vBeta.setZero(nInt);
            vMu.setZero(nInt);

            vEta = material->getEta();
            vTau = material->getTau();
            vBeta = material->getBeta();
            vMu = material->getVMu();
		}
	}

	if(!material->type.compare("thermoelastic"))
	{
		thermoMode = material->getMode();

		thetaVec.setZero(8);

		if(Kond.size() == 0) Kond.setZero(8,8);
	}
}

// ***

void Brick::setMatAtComp(Material* ma)
{
	material = ma;
}

// ***


void Brick::initializeCompRes(const bool& st, const double& grav, const int& gravDir, const bool augLagFlag)
{
	int i;

	Fq.setZero(numDofs);

	integShape.setZero(8);

	calcIntegShape();
	
	if(grav!=0.0)
	{
		int j;
		
		Ft.resize(numDofs);

		for (j=0; j<8;++j) //loop on Shape Functions
		{
			Ft[dim*j+gravDir-1] -= material->getDensity() * grav * integShape(j);
		}
	}
	
	if(st!=0)	//Static or Dynamic
	{
		Fi.resize(numDofs);
		//M.setZero(numDofsInertia, numDofsInertia);
	}
}

// ***

void Brick::initializeCompTang(const bool& st)
{
	//int i;
	Kq.setZero(numDofs, numDofs);
	
	Sgor.setZero(9,9);	// Stress dependent matrix for geometric stiffness 9x9
	
	if(st!=0)
	{
		Ki.setZero(numDofs, numDofs);
	}
}

// ***

void Brick::setQfromq()
{
	//Q is 8x3
	int i;
	int j;

	if(integratorType != 1)
	{
		for (i=0; i < 8 ; ++i)
		{
			if (!material->type.compare("thermoelastic") or !material->type.compare("thermoviscoelastic") )
			{
				thetaVec(i) = q[numDofsInertia + i];
			}

			for (j=0; j < 3 ; ++j)
			{
				Q(i,j) = q[3 * i + j];
			}
		}
	}
	else
	{
		for (i=0; i < 8 ; ++i)
		{
			if (!material->type.compare("thermoelastic") or !material->type.compare("thermoviscoelastic") )
			{
				thetaVec(i) = 0.5 * (q[numDofsInertia + i] + qC[numDofsInertia + i]);
			}

			for (j=0; j < 3 ; ++j)
			{
				Q(i,j) = 0.5 * (q[3 * i + j] + qC[3 * i + j]);
			}
		}
	}
}

// ***
void Brick::makeU()
{
	// U is 24 x 1
	// U = qE - qEref;
	int i;
	int j;
	for (i=0; i<8; ++i)
	{
		for (j=0; j<3; ++j)
		{
			U(3*i+j) = Q(i,j) - Qref(i,j);
		}
	}	
}

// ***

void Brick::makevG()
{
	int i;
	
	for (i=0; i<8; ++i) // Loop on Gauss points
	{
		double xi, eta, zeta;

		xi = gau(i,0);
		eta = gau(i,1);
		zeta = gau(i,2);

		makeJ0(xi, eta, zeta);	// Computation of H, J0 and J0i
		// G' = J0i * H
		// H is 3x8
		// J0i is 3x3
		// G' is 3x8
		// row j of G' contains the derivatives of shape function j relative to X,Y,Z
		vG.block(3*i,0,3,8) = J0i * H;
	}
}

// ***

void Brick::makeG(int num)
{
	int j;
	G.setZero(9,24);
	
	for (j=0; j<8; ++j)
	{
		G.block(0,3*j,3,1) = vG.block(3*num,j,3,1);
		G.block(3,3*j+1,3,1) = vG.block(3*num,j,3,1);
		G.block(6,3*j+2,3,1) = vG.block(3*num,j,3,1);
	}
}

// ***

void Brick::makeTheta()
{
	// the = G*U
	// G is 9x24
	// U is 24x1
	// the is 9x1
	
	the = G * U;
}

// ***

void Brick::makeA()
{
	// Matrix A from vector the
	// A is 6x9
	
	A.setZero(6,9);
	
	A(0,0) = the(0);
	A(0,3) = the(3);
	A(0,6) = the(6);
	
	A(1,1) = the(1);
	A(1,4) = the(4);
	A(1,7) = the(7);
	
	A(2,2) = the(2);
	A(2,5) = the(5);
	A(2,8) = the(8);
	
	A(3,0) = A(1,1);
	A(3,1) = A(0,0);
	A(3,3) = A(1,4);
	A(3,4) = A(0,3);
	A(3,6) = A(1,7);
	A(3,7) = A(0,6);
	
	A(4,0) = A(2,2);
	A(4,2) = A(0,0);
	A(4,3) = A(2,5);
	A(4,5) = A(0,3);
	A(4,6) = A(2,8);
	A(4,8) = A(0,6);
	
	A(5,1) = A(2,2);
	A(5,2) = A(1,1);
	A(5,4) = A(2,5);
	A(5,5) = A(1,4);
	A(5,7) = A(2,8);
	A(5,8) = A(1,7);	
}
// ***

void Brick::makeB()
{
	// B  = (H0+A)*G
	// H0 and A are 6x9
	// G is 9x24
	// B is 6x24
	
	B = (H0 + A) * G;
}

// ***
	
void Brick::makeJ0(double & xi, double & eta, double & zeta )
{

	H(0,0) = -(1.0 - eta) * (1.0 - zeta) / 8.0;
	H(0,1) = (1.0 - eta) * (1.0 - zeta) / 8.0;
	H(0,2) = (1.0 + eta) * (1.0 - zeta) / 8.0;
	H(0,3) = -(1.0 + eta) * (1.0 - zeta) / 8.0;
	H(0,4) = -(1.0 - eta) * (1.0 + zeta) / 8.0;
	H(0,5) = (1.0 - eta) * (1.0 + zeta) / 8.0;
	H(0,6) = (1.0 + eta) * (1.0 + zeta) / 8.0;
	H(0,7) = -(1.0 + eta) * (1.0 + zeta) / 8.0;

	H(1,0) = -(1.0 - xi) * (1.0 - zeta) / 8.0;
	H(1,1) = -(1.0 + xi) * (1.0 - zeta) / 8.0;
	H(1,2) = (1.0 + xi) * (1.0 - zeta) / 8.0;
	H(1,3) = (1.0 - xi) * (1.0 - zeta) / 8.0;
	H(1,4) = -(1.0 - xi) * (1.0 + zeta) / 8.0;
	H(1,5) = -(1.0 + xi) * (1.0 + zeta) / 8.0;
	H(1,6) = (1.0 + xi) * (1.0 + zeta) / 8.0;
	H(1,7) = (1.0 - xi) * (1.0 + zeta) / 8.0;

	H(2,0) = -(1.0 - xi) * (1.0 - eta) / 8.0;
	H(2,1) = -(1.0 + xi) * (1.0 - eta) / 8.0;
	H(2,2) = -(1.0 + xi) * (1.0 + eta) / 8.0;
	H(2,3) = -(1.0 - xi) * (1.0 + eta) / 8.0;
	H(2,4) = (1.0 - xi) * (1.0 - eta) / 8.0;
	H(2,5) = (1.0 + xi) * (1.0 - eta) / 8.0;
	H(2,6) = (1.0 + xi) * (1.0 + eta) / 8.0;
	H(2,7) = (1.0 - xi) * (1.0 + eta) / 8.0;

	// J0 = H*Qref
	// H is 3x8
	// Qref is 8x3
	// J0 is 3x3
    J0 = H * Qref;
	detJ0 = J0.determinant();
    J0i = J0.inverse();
}

// ***

void Brick::makeF()
{
	// F = (J0i*H*Q)^T
	// J0i is 3x3
	// H is 3x8
	// Q is 8x3
	// F is 3x3	

	//F = (J0i * H * Q).transpose();

	F = Q.transpose() *  H.transpose() * J0i.transpose();
}

void Brick::makeC()
{
	// C = (F)^T*F
	// F is 3x3
	// C is 6x1
	CMat = F.transpose() * F;
	VoigtFormat(CMat, C);
}

// ***

void Brick::makeKq(int k)
{
	// Standard integrator
	if(integratorType == 0)
	{
		// MATERIAL contribution of Gauss point = B^T * Ct * B * detJ0
		// B^T is 24x6
		// Ct is linearized constitutive matrix 6x6
		// B is 6x24
		// Contribution is 24x24

		// Hyperelasticity
		if(material->type.compare("viscoelastic") and material->type.compare("thermoelastic") and material->type.compare("damage"))
		{
			if(material->type.compare("svk")) material->calcCt(C);

			Kq += B.transpose() * material->getCt() * B * detJ0;
		}
		// Hyperelasticity with damage
		else if (!material->type.compare("damage"))
		{
			material->calcCt(C, kappa);

			Kq += B.transpose() * material->getCt() * B * detJ0;
		}
		// Viscoelasticity
		else if (!material->type.compare("viscoelastic"))
		{
			if(!viscoMode) //Means internal variables pre-integrated
		    {
				if(material->type.compare("svk"))      material->calcCt(C);

		    	if(!material->hypertype.compare("coupled"))
		    	{
		    		Kq += B.transpose() * (1.0 + delta) * material->getCt() * B * detJ0;
		    	}
		    	else
		    	{
		    		Kq += B.transpose() * ( material->getCtvol() + (1.0 + delta) * material->getCtiso() ) * B * detJ0;
		    	}
		    }
		    else // Means internal variables to be integrated.
		    {
		    	material->calcCt(C,Gam);
		    	Kq += B.transpose() * material->getCt() * B * detJ0;
		    }
		}
		// Thermoelasticity
		else if (!material->type.compare("thermoelastic"))
		{

		}
	}
	// Midpoint Integrator
	if(integratorType == 1)
	{
		// MATERIAL contribution of Gauss point = B^T * Ct * B * detJ0
		// B^T is 24x6
		// Ct is linearized constitutive matrix 6x6
		// B is 6x24
		// Contribution is 24x24

		// Hyperelasticity
		if(material->type.compare("viscoelastic") and material->type.compare("thermoelastic"))
		{
			if(material->type.compare("svk")) material->calcCt(C);

			Kq += 0.5 * B.transpose() * material->getCt() * B * detJ0;

		}
		// Viscoelasticity
		else if (!material->type.compare("viscoelastic"))
		{
    		if(!viscoMode) //Means internal variables pre-integrated
    		{
    			if(material->type.compare("svk"))      material->calcCt(C);

    			if(!material->hypertype.compare("coupled"))
    			{
    				Kq += 0.5 * B.transpose() * (1.0 + delta) * material->getCt() * B * detJ0;
    			}
    			else
    			 {
    				Kq += 0.5 * B.transpose() * ( material->getCtvol() + (1.0 + delta) * material->getCtiso() ) * B * detJ0;
    			 }
    		}
    		else // Means internal variables to be integrated.
    		{
    			material->calcCt(C, Gam);

    			Kq += 0.5 * B.transpose() * material->getCt() * B * detJ0;
         	}
     	}
	}
	// Consistent integrator
	else if(integratorType == 2)
	{
		// MATERIAL contribution of Gauss point = B^T * (Ct + Cross + 2 * alpha *  I )* B * detJ0
		// B^T is 24x6
		// Ct is linearized constitutive matrix 6x6
		// B is 6x24
		// Contribution is 24x24

		// Hyperelasticity
		if(material->type.compare("viscoelastic") and material->type.compare("thermoelastic") and material->type.compare("damage"))
		{
			if(material->type.compare("svk")) material->calcCt(CMid); // means not to be svk

			if(DeltaCDeltaC != 0.0)
			{
				material->calcS(C);
				Cross = (C - vCConv.col(k)) * ( material->getS() - 0.25 * material->getCt() * makeVoigt(C - vCConv.col(k)) - SMid - 2.0 * alpha * (C - vCConv.col(k)) ).transpose() / DeltaCDeltaC;
			}
			else Cross.setZero(6,6);

			Kq += 0.5 * (B + vBConv.block(6*k,0,6,24)).transpose() * (0.5 * material->getCt() + 2.0 * Cross + 2.0 * alpha * I) * B * detJ0;

		}
		//Hyperelasticity with damage
		if(!material->type.compare("damage"))
		{

		}
		// Viscoelasticity
		if(!material->type.compare("viscoelastic"))
		{
			if(material->type.compare("svk")) material->calcCt(CMid,Gam); // Remind that the Gam-dependent is not programmed so any Gam value can be used.

			if(DeltaCDeltaC != 0.0)
			{
 				material->calcS(C, Gam);
 				Cross =  (C - vCConv.col(k)) * 0.5 * material->getS().transpose();

 				material->calcS(C, vGamConv.col(k));
 				Cross += (C - vCConv.col(k)) * 0.5 * material->getS().transpose();

 				material->calcCt(CMid,Gam);
 				Cross -= (C - vCConv.col(k)) * ( 1.0 / 8.0 * material->getCt() * makeVoigt(C - vCConv.col(k)) ).transpose();

 				material->calcCt(CMid,vGamConv.col(k));
 				Cross += (C - vCConv.col(k)) * ( 1.0 / 8.0 * material->getCt() * makeVoigt(C - vCConv.col(k)) - SMid - 2.0 * alpha * (C - vCConv.col(k)) ).transpose();

 				Cross /= DeltaCDeltaC;
        	}
			else Cross.setZero(6,6);

 			//material->calcCt(CMid,Gam);
 			Kq += 0.5 * (B + vBConv.block(6*k,0,6,24)).transpose() * ( 0.25 * material->getCt() + 2.0 * Cross + 2.0 * alpha * I) * B * detJ0;

 			material->calcCt(CMid,vGamConv.col(k));
 			Kq += 0.5 * (B + vBConv.block(6*k,0,6,24)).transpose() * 0.25 * material->getCt() * B * detJ0;
		}
		//Thermoelasticity
		if(!material->type.compare("thermoelastic"))
		{

		}
}

	// GEOMETRICAL contribution of Gauss point = G^T * Sgor * G * detJ0
	// G^T is 24x9
	// Sgor is a stress-dependent matrix 9x9
	// G is 9x24
	// Contribution is 24x24
	// Diagonal

	int m, n;
	  
	for(m=0; m<dim; ++m)
	{
		for(n=0; n<dim; ++n)
		{
			Sgor(3*m+n,3*m+n) = S(n);
		}
	}
		
	for(m=0; m<dim; ++m)
	{
		Sgor(3*m,3*m+1) = S(3);
		Sgor(3*m,3*m+2) = S(4);
		Sgor(3*m+1,3*m+2) = S(5);
		
		Sgor(3*m+1,3*m) = S(3);
		Sgor(3*m+2,3*m) = S(4);
		Sgor(3*m+2,3*m+1) = S(5);
	}

	if(integratorType == 0)
	{
		Kq += G.transpose() * Sgor * G * detJ0; 
	}
	else
	{
		Kq += 0.5 * G.transpose() * Sgor * G * detJ0;
	}
// 	cout << "Kq= " << endl << Kq << endl;
}

// ***

void Brick::calcHv(int i,int k)
{
	Hv.segment<6>(6*i) = exp(- deltaT / (2.0 * vTau[i]) ) * (exp(- deltaT / (2.0 * vTau[i]) ) * vQvConv.block(6*i,k,6,1) - vBeta[i] * vSConv.col(k));
}

// ***

void Brick::calcQv(int i,int k)
{
	if(!material->hypertype.compare("coupled"))
	{
		material->calcS(C);
		Qv.segment<6>(6*i) = vBeta[i] * exp( - deltaT / (2.0 * vTau[i]) ) * material->getS() + Hv.segment<6>(6*i);
		
		// Gam.segment<6>(6*i) = 2.0 /  material->getE() * material->getS(C) - Qv.segment<6>(6*i) / (vBeta[i] * material->getE());
	}
	else
	{
		material->calcSiso(C);
		Qv.segment<6>(6*i) = vBeta[i] * exp( - deltaT / (2.0 * vTau[i]) ) * material->getSiso() + Hv.segment<6>(6*i);
		
		//Gam.segment<6>(6*i) = 2.0 /  material->getE() * material->getS_Iso(C) - Qv.segment<6>(6*i) / (vBeta[i] * material->getE());
	}
}

// ***

void Brick::calcQvConsistently(int k)
{
	// cout << "N= " << nInt << endl;
    switch (nInt)
    {
        case 1:

        	GamMid = 0.5 * (Gam + vGamConv.col(k));

        	material->calcQv(0,C, GamMid);
            QvMid = 0.5 * material->getQv();

            material->calcQv(0,vCConv.col(k),GamMid);
            QvMid += 0.5 * material->getQv();
	
            QvMidDeltaGam = dotVoigt(QvMid, Gam - vGamConv.col(k));
	
            DeltaGamDeltaGam = dotVoigt(Gam - vGamConv.col(k), Gam - vGamConv.col(k));
	
            if(DeltaGamDeltaGam != 0.0)
            {
            	material->calcW(C, Gam);
                beta = - 0.5 * material->getW();

                material->calcW(C,vGamConv.col(k));
                beta += 0.5 * material->getW();

                material->calcW(vCConv.col(k),Gam);
                beta -= 0.5 * material->getW();

                beta += 0.5 * vWConv(k);
                beta -= QvMidDeltaGam;

                beta /= DeltaGamDeltaGam;
            }
            else beta = 0.0;

            Qv = QvMid + beta * (Gam - vGamConv.col(k));

            Qv /= vEta[0];

            // Tangent Matrix

            if (DeltaGamDeltaGam != 0.0)
            {
            	material->calcQv(0,C,GamMid);
            	CrossKg = 0.5 * material->getQv();

            	material->calcQv(0,vCConv.col(k),GamMid);
  				CrossKg += 0.5 * material->getQv() + vEta[0] / (2.0 * vTau[0]) * (Gam - vGamConv.col(k)) - QvMid - 2.0 * beta * (Gam - vGamConv.col(k));

  				CrossKg /= DeltaGamDeltaGam;
            }
            else CrossKg.setZero(6);

            Kiv = (1.0 - deltaT * beta / vEta[0] + deltaT / 2.0 / vTau[0]) * MatrixXd::Identity(6,6) - deltaT / vEta[0] * (Gam - vGamConv.col(k)) * CrossKg.transpose();

            break;

        case 2:

        	// Qv1: -Dg_1 (Psi)

        	GamMid.segment(0,6) = 0.5 * (Gam.segment(0,6) + vGamConv.col(k).segment(0,6));
        	GamMid.segment(6,6) = Gam.segment(6,6);

        	material->calcQv(0, vCConv.col(k), GamMid);
            QvMid.segment(0,6) = 0.5 * material->getQv();

        	GamMid.segment(0,6) = 0.5 * (Gam.segment(0,6) + vGamConv.col(k).segment(0,6));
        	GamMid.segment(6,6) = vGamConv.col(k).segment(6,6);

            material->calcQv(0,C,GamMid);
            QvMid.segment(0,6) += 0.5 * material->getQv();

            QvMidDeltaGam = dotVoigt(QvMid.segment(0,6), (Gam - vGamConv.col(k)).segment(0,6));

            DeltaGamDeltaGam = dotVoigt((Gam - vGamConv.col(k)).segment(0,6), (Gam - vGamConv.col(k)).segment(0,6));

            if(DeltaGamDeltaGam != 0.0)
            {
            	material->calcW(vCConv.col(k), Gam);
                beta = - 0.5 * material->getW();

                auxGam.segment(0,6) = vGamConv.col(k).segment(0,6);
                auxGam.segment(6,6) = Gam.segment(6,6);

                material->calcW(vCConv.col(k),auxGam);
                beta += 0.5 * material->getW();

                auxGam.segment(0,6) = Gam.segment(0,6);
                auxGam.segment(6,6) = vGamConv.col(k).segment(6,6);

                material->calcW(C,auxGam);
                beta -= 0.5 * material->getW();

                material->calcW(C, vGamConv.col(k));
                beta += 0.5 * material->getW() - QvMidDeltaGam;

                beta /= DeltaGamDeltaGam;
            }
            else beta = 0.0;

            Qv.segment(0,6) = QvMid.segment(0,6) + beta * (Gam - vGamConv.col(k)).segment(0,6);

            Qv.segment(0,6) /= vEta[0];

            // First block of tangent Matrix

            if (DeltaGamDeltaGam != 0.0)
            {
            	material->calcQv(0,C,GamMid);
            	CrossKg = 0.5 * material->getQv();

            	material->calcQv(0,vCConv.col(k),GamMid);
  				CrossKg += 0.5 * material->getQv() + vEta[0] / (2.0 * vTau[0]) * (Gam - vGamConv.col(k)).segment(0,6) - QvMid.segment(0,6) - 2.0 * beta * (Gam - vGamConv.col(k)).segment(0,6);

  				CrossKg /= DeltaGamDeltaGam;

  				// dQ1 / dGam2
  	            material->calcQv(1,vCConv.col(k),Gam);
  	            auxQv = -material->getQv();

  	            auxGam.segment(0,6) = vGamConv.col(k).segment(0,6);
  	            auxGam.segment(6,6) = Gam.segment(6,6);
  	            material->calcQv(1,vCConv.col(k),auxGam);
  	            auxQv += material->getQv();

  	            Kiv.block(0,6,6,6) = 0.5 * (Gam - vGamConv.col(k)).segment(0,6) * auxQv.transpose() / DeltaGamDeltaGam;
            }
            else CrossKg.setZero(6);

            Kiv.block(0,0,6,6) = (1.0 - deltaT * beta / vEta[0] + deltaT / 2.0 / vTau[0]) * MatrixXd::Identity(6,6) - deltaT / vEta[0] * (Gam - vGamConv.col(k)).segment(0,6) * CrossKg.transpose();

            // Qv2: - Dg2 (Psi): Second discrete derivative

            GamMid.segment(0,6) = vGamConv.col(k).segment(0,6);
            GamMid.segment(6,6) = 0.5 * (Gam.segment(6,6) + vGamConv.col(k).segment(6,6));

            material->calcQv(1, vCConv.col(k), GamMid);
            QvMid.segment(6,6) = 0.5 * material->getQv();

            GamMid.segment(0,6) = Gam.segment(6,6);
            GamMid.segment(6,6) = 0.5 * (Gam.segment(6,6) + vGamConv.col(k).segment(6,6));

            material->calcQv(1,C,GamMid);
            QvMid.segment(6,6) += 0.5 * material->getQv();

            QvMidDeltaGam = dotVoigt(QvMid.segment(6,6), (Gam - vGamConv.col(k)).segment(6,6));

            DeltaGamDeltaGam = dotVoigt((Gam - vGamConv.col(k)).segment(6,6), (Gam - vGamConv.col(k)).segment(6,6));

            if(DeltaGamDeltaGam != 0.0)
            {
                auxGam.segment(0,6) = vGamConv.col(k).segment(0,6);
                auxGam.segment(6,6) = Gam.segment(6,6);

            	material->calcW(vCConv.col(k), auxGam);
                beta = - 0.5 * material->getW();

                beta += 0.5 * vWConv(k);

                material->calcW(C,Gam);
                beta -= 0.5 * material->getW();

                auxGam.segment(0,6) = Gam.segment(0,6);
                auxGam.segment(6,6) = vGamConv.col(k).segment(6,6);

                material->calcW(C, auxGam);
                beta += 0.5 * material->getW() - QvMidDeltaGam;

                beta /= DeltaGamDeltaGam;
            }
            else beta = 0.0;

            Qv.segment(6,6) = QvMid.segment(6,6) + beta * (Gam - vGamConv.col(k)).segment(6,6);

            Qv.segment(6,6) /= vEta[1];

            // Second block of Tangent Matrix
            if (DeltaGamDeltaGam != 0.0)
            {
            	material->calcQv(1,vCConv.col(k),GamMid);
            	CrossKg = 0.5 * material->getQv();

            	material->calcQv(1,vCConv.col(k),GamMid);
  				CrossKg += 0.5 * material->getQv() + vEta[1] / (2.0 * vTau[1]) * (Gam - vGamConv.col(k)).segment(6,6) - QvMid.segment(6,6) - 2.0 * beta * (Gam - vGamConv.col(k)).segment(6,6);

  				CrossKg /= DeltaGamDeltaGam;

  				// dQ2 / dGam1
  	            material->calcQv(0,C,Gam);
  	            auxQv = -material->getQv();

  	            auxGam.segment(0,6) = Gam.segment(0,6);
  	            auxGam.segment(6,6) = vGamConv.col(k).segment(6,6);
  	            material->calcQv(0,C,auxGam);
  	            auxQv += material->getQv();

  	            Kiv.block(6,0,6,6) = 0.5 * (Gam - vGamConv.col(k)).segment(0,6) * auxQv.transpose() / DeltaGamDeltaGam;
            }
            else CrossKg.setZero(6);

            Kiv.block(6,6,6,6) = (1.0 - deltaT * beta / vEta[1] + deltaT / 2.0 / vTau[1]) * MatrixXd::Identity(6,6) - deltaT / vEta[1] * (Gam - vGamConv.col(k)).segment(6,6) * CrossKg.transpose();

            break;

        case 3:
        	cout << "**ERROR: Consistent integration is only available up to 2 internal variables" << endl;
        	break;
    }
   // cout << "Qv("<< k+1 <<")= "<< endl << Qv << endl;
   // cout << "Kiv("<< k+1 <<")= " << endl << Kiv << endl;
}

// ***

void Brick::integrateInternalStrains(int k, int withk)
{
    // k being the Gauss point

	int i,numIter,maxIter;
	VectorXd deltaGam;
	double resmod;
	double tolGam = 1e-10;
	
	if(resGam.size()==0) resGam.setZero(6*nInt);
    if(Kiv.size()==0) Kiv.setZero(6*nInt,6*nInt);
	
	maxIter = 30;
	
	Gam = vGamConv.col(k);
	
	// Standard integrators
	if (integratorType == 0)
	{
        deltaGam.setZero(6*nInt);
        numIter = 0;
        resmod = 1.0;
        while(resmod >= tolGam)
        {
            Gam += deltaGam;
            
            for(i=0;i<nInt;++i) // Loop on internal variables
            {
            	material->calcQv(i,C,Gam);
                Qv.segment<6>(6*i) = 1.0 / ( 2.0 * vEta[i] ) * (material->getQv() + vQvConv.block(6*i,k,6,1));
                //material->getQv(i,vCConv.col(k),vGamConv.col(k)));

                if (withk == 0)
                {
                    Kiv.block(6*i,6*i,6,6)  = (1.0 + deltaT / 2.0 / vTau[i]) * MatrixXd::Identity(6,6);
                    // Kiv.block(3,3,3,3) *= 0.5;
                }
                else
                {
                    calcKivNumeric(i,k);
                }
            }
    
            resGam = Gam - vGamConv.col(k) - deltaT * Qv;
				
            resmod = resGam.norm();
				
            deltaGam = - Kiv.colPivHouseholderQr().solve(resGam);

            if(numIter == maxIter)
            {
                break;
                cout << "*ERROR: Convergence in Internal Variables did not achieve at maxIter =" << maxIter << endl;
            }
            numIter +=1;
        }
	}
	// Midpoint Integrator
	else if (integratorType == 1)
	{
        deltaGam.setZero(6*nInt);
        numIter = 0;
        resmod = 1.0;
        while(resmod >= tolGam)
        {
            for(i=0;i<nInt;++i) // Loop on internal variables
            {
            	material->calcQv(i,C,Gam);
                Qv.segment<6>(6*i) = 1.0 / vEta[i] * material->getQv();
                
                if (withk == 0)
                {
                    Kiv.block(6*i,6*i,6,6)  = (1.0 + deltaT / 2.0 / vTau[i]) * MatrixXd::Identity(6,6);
                }
                else
                {
                    calcKivNumeric(i,k);
                }
            }
            
            Gam += deltaGam;
				
            resGam = Gam - vGamConv.col(k) - deltaT * Qv;
				
            resmod = resGam.norm();
			
            deltaGam = - Kiv.colPivHouseholderQr().solve(resGam);
			
            if(numIter == maxIter)
            {
                break;
                cout << "*ERROR: Convergence in Internal Variables did not achieve at maxIter =" << maxIter << endl;
            }
            numIter +=1;
        }
	}
	// Consistent integrator
	else if (integratorType == 2 or integratorType == 3)
	{
		deltaGam.setZero(6*nInt);
		numIter = 0;
		resmod = 1.0;

        while(resmod >= tolGam)
        {
            Gam += deltaGam;
            calcQvConsistently(k); 	//k is the Gauss point number where we want to calculate Gam.
                        
//          cout << "Gam= " << Gam << endl;
//          cout << "GamConv= " << vGamConv.col(k) << endl;
//          //cout << "Qv= " << Qv << endl;

            resGam = Gam - vGamConv.col(k) - deltaT * Qv;
		
            resmod = resGam.norm();

			//calcKivConsistently(k,0);
			
			deltaGam = - Kiv.colPivHouseholderQr().solve(resGam);
			
			if(numIter == maxIter)
			{
				break;
				cout << "*ERROR: Convergence in Internal Variables did not achieve at maxIter =" << maxIter << endl;
			}
			numIter +=1;
		}
//		cout << "Gam= " << endl << Gam << endl;
	}
// 		cout << "Checking convergence on Internal Variables in Gauss point number " << k << "..." << endl;
// 		if(resmod >tolGam) cout << "\t" << "It Has NOT converged " << "\t" << "ResIntVar= " << resmod << endl;
// 		else                 cout << "\t" << "Covergence has GOT" << "\t" << "ResIntVar= " << resmod << endl;

 	//	cout << "Gam("<< k+1 <<")= "<< endl << Gam << endl;
 	//	cout << "GamConv("<<k<<")= "<< endl << vGamConv.col(k) << endl;
}

//***

void Brick::calcKivNumeric(int i, int k)
{
        //Being i the internal variable number
        //Being k the Gauss point
        
	if(Kiv.size()==0) Kiv.setZero(6*nInt,6*nInt);

    if (integratorType == 0)
    {
    	int j;
    	for (j=0;j<6; ++j)
    	{
    		Gam(6 * i + j) += perturbation;
    		material->calcQv(i,C,Gam);
    		Kiv.col(j) = Gam.segment<6>(6*i) - vGamConv.block(6*i,k,6,1) - deltaT / ( 2.0 * vEta[i] ) * (material->getQv() + vQvConv.block(6*i,k,6,1));

            Gam(6 * i + j) -= 2.0 * perturbation;
            material->calcQv(i,C,Gam);
            Kiv.col(j) -= (Gam.segment<6>(6*i) - vGamConv.block(6*i,k,6,1) - deltaT / ( 2.0 * vEta[i] ) * (material->getQv() +  vQvConv.block(6*i,k,6,1)));
				
            Gam(6 * i + j) += perturbation;
        }
        Kiv /= (2.0 * perturbation);
        //		cout << "Kiv= " << endl << Kiv << endl << endl;
    }
    else if (integratorType == 1)
    {
    	int j;
    	for (j=0;j<6; ++j)
        {
    		Gam(6 * i + j) += perturbation;
    		material->calcQv(i,C,Gam);
            Kiv.col(j) = Gam.segment<6>(6*i) - vGamConv.block(6*i,k,6,1) - deltaT / vEta[i] * material->getQv();

            Gam(6 *i + j) -= 2.0 * perturbation;
            material->calcQv(i,C,Gam);
            Kiv.col(j) -= (Gam.segment<6>(6*i) - vGamConv.block(6*i,k,6,1) - deltaT / vEta[i] * material->getQv());

            Gam(6 * i + j) += perturbation;
        }
    	Kiv /= (2.0 * perturbation);
//    	cout << "Kiv= " << endl << Kiv << endl << endl;
        }
}

//void Brick::calcKivConsistently(int k, int withk)
//{
//        if(Kiv.size()==0) Kiv.setZero(6*nInt,6*nInt);
//
//        if(withk == 0)
//        {
//        	switch(nInt)
//            {
//        		case 1:
//
//        			if (CrossKg.size() == 0) CrossKg.setZero(6);
//
//                    if (DeltaGamDeltaGam != 0.0)
//                    {
//                    	material->calcQv(nInt-1,C,GamMid);
//                    	CrossKg = 0.5 * material->getQv();
//
//                    	material->calcQv(nInt-1,vCConv.col(k),GamMid);
//          				CrossKg += 0.5 * material->getQv() + vEta[0] / (2.0 * vTau[0]) * (Gam - vGamConv.col(k)) - QvMid - 2.0 * beta * (Gam - vGamConv.col(k));
//
//          				CrossKg /= DeltaGamDeltaGam;
//                    }
//                    else CrossKg.setZero(6);
//
//                    Kiv = (1.0 - deltaT * beta / vEta[0] + deltaT / 2.0 / vTau[0]) * MatrixXd::Identity(6,6) - deltaT / vEta[0] * (Gam - vGamConv.col(k)) * CrossKg.transpose();
//
//                    break;
//
//        		case 2:
//
////        			for(int i=1 ; i<= nInt ; ++i) // Loop on internal variables
////                    {
//        				cout << "Kiv to be done in this case" << endl;
////                    }
//                    break;
//             }
//
//        }
//        else if(withk == 1)
//        {
//        	int i, j;
//            for (i=0;i<nInt;i++)
//            {
//            	for (j=0;j<6; ++j)
//            	{
//            		Gam(6 * i + j) += perturbation;
//            		calcQvConsistently(k);
//            		Kiv.col(j) = Gam.segment<6>(6*i) - vGamConv.block(6*i,k,6,1) - deltaT / vEta[i] * Qv.segment<6>(6*i);
//
//            		Gam(6 * i + j) -= 2.0 * perturbation;
//            		calcQvConsistently(k);
//                    Kiv.col(j) -= Gam.segment<6>(6*i) - vGamConv.block(6*i,k,6,1) - deltaT / vEta[i] * Qv.segment<6>(6*i);
//
//                    Gam(6 * i + j) += perturbation;
//            	}
//
//            	Kiv /= (2.0 * perturbation);
//            }
//            //cout << "Kiv= " << endl << Kiv << endl;
//        }
//}

// ***

void Brick::integrateDamage(int k, int withK)
{

}

// ***

void Brick::calcEner(double& Ekin, double& Epot, double& Econ, double& Eext, double& Eamb, double& Dint, double& grav, int& gravDir, vector<double>& q_in, vector<double>& dq_in)
{
	int i;
	int j;
	double zg = 0.0;
	double stEnermid;
    double dissNewmark;
	
	// Kinetic energy
	if(dq.size()!=0 && (M.size()!=0 || MLumped.size()!=0) )
	{
		for (i=0; i<numDofs; ++i)	dq[i] = dq_in[localGlob[i]];
		for (i=0; i<numDofsInertia; ++i)
		{
			if (MLumped.size()==0)	// Consistent mass matrix
			{
				for (j=0; j<numDofsInertia; ++j)
				{
					Ekin+= 0.5*dq[i] * M(i,j) * dq[j];
				}
			}
			else	// Lumped mass matrix
			{
				Ekin+= 0.5 * MLumped[i] * dq[i] * dq[i];
			}
		}
	}
	// Strain energy
	for (i=0; i<numDofs; ++i)	q[i] = q_in[localGlob[i]];

	if (integratorType == 0) // means newmark integrator
	{
		int k;
        dissNewmark = 0.0;
        
        if(!material->type.compare("viscoelastic")) // means viscoelastic models
        {
        	for (i=0; i < 8 ; ++i)
            {
        		// if(qC.size() == 0) qC = q;
        		for (j=0; j < 3 ; ++j)
        			Q(i,j) = 0.5 * (q[3 * i + j] + qC[3 * i + j]);
            }

        	for (k=0; k<8; ++k)     // Loop on Gauss points
            {
        		double xi, eta, zeta;

        		xi = gau(k,0);
                eta = gau(k,1);
                zeta = gau(k,2);

                makeJ0(xi, eta, zeta);   // Computation of H, J0 and J0i
                makeF();
                makeC();
                
                if(!viscoMode)
                {
                	for(i=0;i<nInt;++i)
                    {
                		calcHv(i,k);
                        calcQv(i,k);
                        dissNewmark += dotVoigt(Qv.segment<6>(6*i), Gam.segment<6>(6*i) - vGamConv.block(6*i,k,6,1)) * detJ0;
                    }
                }
                else
                {
                	integrateInternalStrains(k,0);

                	for(i=0;i<nInt;++i)
                    {
                		material->calcQv(i,C,Gam);
                        dissNewmark +=  dotVoigt(material->getQv(), Gam.segment<6>(6*i) - vGamConv.block(6*i,k,6,1)) * detJ0;
                    }
                }
            }
   }
                
                Epot += stEner;
                Dint += dissNewmark;
	}
	else if(integratorType == 1)	//means midpoint
	{  
		int k;
		stEnermid = 0.0;

		for (i=0; i < 8 ; ++i)
		{
			if(!material->type.compare("thermoelastic") or !material->type.compare("thermoelastic"))
			{
				thetaVec(i) = q[numDofsInertia + i];
			}

			for (j=0; j < 3 ; ++j)	Q(i,j) = q[3 * i + j];
		}
	
		for (k=0; k<8; ++k)	// Loop on Gauss points
		{
        	double xi, eta, zeta;

        	xi = gau(k,0);
        	eta = gau(k,1);
        	zeta = gau(k,2);

			makeJ0(xi, eta, zeta);	// Computation of H, J0 and J0i
			makeF();
			makeC();
		
			//Hyperelasticity
			if(material->type.compare("viscoelastic") and material->type.compare("themoelastic") and material->type.compare("damage"))
			{
				material->calcW(C);
				stEnermid += material->getW() * detJ0;
			}
			// Hyperelasticity with damage
			else if (!material->type.compare("damage"))
			{
				material->calcW(C,kappa);
				stEnermid += material->getW() * detJ0;
			}
			//Viscoelasticty
			else if (!material->type.compare("viscoelastic")) // means viscoelastic model
			{
                if(!viscoMode)
                {
                	material->calcW(C);
                    stEnermid += material->getW() * detJ0;
                }
                else
                {
                	integrateInternalStrains(k,0);
                	material->calcW(C,Gam);
                    stEnermid += material->getW() * detJ0;
                }
			}
			// Thermoelasticity
			else if (!material->type.compare("thermoelastic"))
			{
        		//Evaluating temperature/entropy at gauss point
        		Theta = Nt.row(k) * thetaVec;

        		material->calcW(C,Theta);
        		stEner += material->getW() * detJ0;
			}
		}
		
        Epot += stEnermid;
        Dint += dissEner;
	}
	else //means Consistent integrator
    {
		Epot += stEner;
        Dint += dissEner;
    }
	// Gravitational potential energy
	if (grav!=0.0)
	{
		for (i=0; i<8; ++i) 
		{
			zg += integShape(i) * q[dim * i + gravDir - 1];
		}
		Epot += grav * material->getDensity() * zg;
	}
}

// ***

void Brick::calcEntropy(double& Ent, double& EntAmb)
{
	int i,k;
	double EntropyMid;

	if(integratorType == 1)
	{
		for( i=0; i<8; ++i)
		{
			thetaVec(i) = q[numDofsInertia + i];
		}

		for ( k=0; k<8; ++k)	// Loop on Gauss points
		{
			double xi, eta, zeta;

			xi = gau(k,0);
			eta = gau(k,1);
			zeta = gau(k,2);

			makeJ0(xi, eta, zeta);	// Computation of H, J0 and J0i
			makeF();
			makeC();

			// Thermoelasticity
			if (!material->type.compare("thermoelastic"))
			{
				Theta = Nt.row(k) * thetaVec;

				material->calcEntropy(C,Theta);
				EntropyMid += material->getEntropy();
			}
		}
		Ent += EntropyMid;
	}
	else
	{
		Ent += Entropy;
	}
}

// ***

void Brick::calcAM(vector<double>& am, vector<double>& q_in, vector<double>& dq_in)
{
	int i, j;
	for (i=0; i<numDofs; ++i)
	{
		q[i] = q_in[localGlob[i]];
		dq[i] = dq_in[localGlob[i]];
	}
	for (i=0; i<8; ++i)
	{
		for (j=0; j<8; ++j)
		{
			am[0] += q[3*i+1] * M(i*3,j*3)*dq[3*j+2] - q[3*i+2] * M(i*3,j*3) * dq[3*j+1];
			am[1] += q[3*i+2] * M(i*3,j*3)*dq[3*j] - q[3*i] * M(i*3,j*3) * dq[3*j+2];
			am[2] += q[3*i] * M(i*3,j*3) * dq[3*j+1] - q[3*i+1] * M(i*3,j*3) * dq[3*j];
		}
	}
}	

// ***

void Brick::updateComp(void)
{
	// Standard integrator
	if (integratorType == 0)
	{
		// Viscoelasticity
		if(!material->type.compare("viscoelastic"))
		{
            if(vCConv.size() == 0)
            {
                vCConv.setZero(6,8);
                vCConv.block(0,0,3,8) << 1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,
                                         1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,
                                         1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0;
            }

			if(!viscoMode) //Means internal variables pre-integrated
			{
				if(vSConv.size() == 0)	vSConv.setZero(6,8);
				if(Qv.size() == 0)	Qv.setZero(6*nInt);
				if(vQvConv.size() == 0)	vQvConv.setZero(6*nInt,8);
				if(Hv.size() == 0)	Hv.setZero(6*nInt);

				setQfromq();

				int k;
				for(k=0; k<8; ++k) //Loop on Gauss Point
				{
	            	double xi, eta, zeta;

	            	xi = gau(k,0);
	            	eta = gau(k,1);
	            	zeta = gau(k,2);

					makeJ0(xi, eta, zeta);	// Computation of H, J0 and J0i
					makeF();
					makeC();

					if(!material->hypertype.compare("coupled"))
					{
						material->calcS(C);
						vSConv.col(k) = material->getS();
					}
					else
					{
						material->calcSiso(C);
						vSConv.col(k) = material->getS();
					}

					for(int i=0; i<nInt; ++i) //Loop on internal variables
					{
						calcHv(i,k); //i means the internal variable number
						calcQv(i,k);
						vQvConv.col(k) = Qv;
					}
					vCConv.col(k) = C;
				}
			}
			else //Means internal variables to be integrated
			{
	            if(Gam.size() == 0)           Gam.setZero(6*nInt);
	            if(vGamConv.size() == 0)      vGamConv.setZero(6*nInt,8);
	            if(Qv.size() == 0)	Qv.setZero(6*nInt);
	            if(vQvConv.size() == 0) vQvConv.setZero(6*nInt, 8);

	            setQfromq();

	            int k;
	            for(k=0; k<8; ++k) //Loop on Gauss Points
	            {
	            	double xi, eta, zeta;

	            	xi = gau(k,0);
	            	eta = gau(k,1);
	            	zeta = gau(k,2);

					makeJ0(xi, eta, zeta);	// Computation of H, J0 and J0i
	                makeF();
	                makeC();
	                integrateInternalStrains(k,0);
	                //cout << "Gam= " << endl << Gam << endl;
	                vGamConv.col(k) = Gam;
	                vCConv.col(k) = C;

	                for(int i=0; i<nInt; ++i)	//Loop on internal variables
	                {
	                	material->calcQv(i,C, Gam);
	                	vQvConv.block(6*i,k,6,1) = material->getQv();
	                }
	            }
			}

		}
		// Thermoelasticity
		if(!material->type.compare("thermoelastic"))
		{
			if(thermoMode) // State variable: Temperature
			{
				if(vEntConv.size() == 0)	vEntConv.setZero(8);
				if(vdEntConv.size() == 0)	vdEntConv.setZero(8);

				setQfromq();
				for(int k=0; k<8; ++k) //Loop on Gauss Points
				{
					double xi, eta, zeta;

					xi = gau(k,0);
					eta = gau(k,1);
					zeta = gau(k,2);

					makeJ0(xi, eta, zeta);	// Computation of H, J0 and J0i
					makeF();
					makeC();

					Theta = 0.0;
					for (int i=0; i<8;++i) // Loop on shape function
						Theta += calcShape(i,xi,eta,zeta) * q[numDofsInertia + i];
					//Theta =  Nt.row(k) * thetaVec;

					material->calcEntropy(C, Theta);

					vdEntConv(k) = 2.0 / deltaT * (material->getEntropy() - vEntConv(k)) - vdEntConv(k);
					vEntConv(k) = material->getEntropy();
				}
			}
			else
			{

			}
		}
	}
	// Midpoint integrator
	if (integratorType == 1)
	{
		// Viscoelasticity
		if(!material->type.compare("viscoelastic"))
		{
			if(!viscoMode) //Means internal variables pre-integrated
			{
				if(vCConv.size() == 0)
				{
					vCConv.setZero(6,8);
					vCConv.block(0,0,3,8) << 1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,
		    							 	 1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,
		    							 	 1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0;
		    	}
		    	if(Qv.size() == 0)	Qv.setZero(6*nInt);
		    	if(vQvConv.size() == 0)	vQvConv.setZero(6*nInt,8);
		    	if(Hv.size() == 0)	Hv.setZero(6*nInt);

		    	setQfromq();

		    	int k;
		    	for(k=0; k<8; ++k) //Loop on Gauss Point
		    	{
		    		double xi, eta, zeta;

		    		xi = gau(k,0);
		            eta = gau(k,1);
		            zeta = gau(k,2);

		    		makeJ0(xi, eta, zeta);	// Computation of H, J0 and J0i
		    		makeF();
		    		makeC();

		    		for(int i=0; i<nInt; ++i)
		    		{
		    			calcHv(i,k); //i means the internal variable number
		    			calcQv(i,k);
		    			vQvConv.block(6*i,k,6,1) = Qv.segment<6>(6*i);
		    		}
		    		vCConv.col(k) = C;
		    	}
		    }
		    else //Means internal variables to be integrated
		    {
		        if(Gam.size() == 0)         Gam.setZero(6*nInt);
		        if(vGamConv.size() == 0)    vGamConv.setZero(6*nInt,8);
		        if(Qv.size() == 0)			Qv.setZero(6*nInt);

		        setQfromq();

		        int k;
		        for(k=0; k<8; ++k) //Loop on Gauss Point
		        {
		    		double xi, eta, zeta;

		    		xi = gau(k,0);
		            eta = gau(k,1);
		            zeta = gau(k,2);

		    		makeJ0(xi, eta, zeta);	// Computation of H, J0 and J0i
		            makeF();
		            makeC();
		            integrateInternalStrains(k,0);
		            //cout << "Gam= " << endl << Gam << endl;
		            vGamConv.col(k) = Gam;
		         }
		    }
		}
	}
	// Consistent integrator
	if (integratorType == 2)
	{
		// Hyperelasticity
		if(material->type.compare("viscoelastic") and material->type.compare("thermoelastic") and material->type.compare("damage"))
        {
			if(vWConv.size() == 0)  vWConv.setZero(8);
			if(CMid.size() == 0)    CMid.setZero(6);        // Right Cauchy-Green Deformation Tensor at mid-point
			if(vBConv.size() == 0)  vBConv.setZero(48,24);	//
            if(vCConv.size() == 0)
            {
            	vCConv.setZero(6,8);
//                vCConv.block(0,0,3,8) << 1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,
//                                         1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,
//                                         1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0;
            }	//
            if(SMid.size() == 0)    SMid.setZero(6);        // Second Piola-Kirchhoff stress tensor at mid-ponit (Vector format)
            if(I.size()==0)         I.setZero(6,6);         // Identity fourth-order tensor in voigt format.
            if(Cross.size()==0)     Cross.setZero(6,6);     // Conservative tagent matrix term.
                
            if(I(0,0) != 1.0)
            {
            	I = MatrixXd::Identity(6,6);
            	I.block(3,3,3,3) *= 0.5;
            }

            setQfromq();
            makevG();

            int k;
            for(k=0; k<8; ++k) //Loop on Gauss Point
            {
	    		double xi, eta, zeta;

	    		xi = gau(k,0);
	            eta = gau(k,1);
	            zeta = gau(k,2);

            	makeG(k);
            	makeTheta();
            	makeA();
            	makeB();
            	makeJ0(xi, eta, zeta);   // Computation of H, J0 and J0i
                makeF();
                makeC();

                vCConv.col(k) = C;
                vBConv.block(6*k,0,6,24) = B;

                material->calcW(C);
                vWConv(k) = material->getW();
            }
        }
		// Hyperelasticity with damage (at the moment equal to hyperelasticity)
		else if (!material->type.compare("damage"))
		{
			if(vWConv.size() == 0)  vWConv.setZero(8);
			if(CMid.size() == 0)    CMid.setZero(6);        // Right Cauchy-Green Deformation Tensor at mid-point
			if(vBConv.size() == 0)  vBConv.setZero(48,24);	//
			if(vCConv.size() == 0)
			{
				vCConv.setZero(6,8);
//                vCConv.block(0,0,3,8) << 1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,
//                                         1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,
//                                         1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0;
			}
			if(SMid.size() == 0)    SMid.setZero(6);        // Second Piola-Kirchhoff stress tensor at mid-ponit (Vector format)
			if(I.size()==0)         I.setZero(6,6);         // Identity fourth-order tensor in voigt format.
			if(Cross.size()==0)     Cross.setZero(6,6);     // Conservative tagent matrix term.

			if(I(0,0) != 1.0)
			{
				I = MatrixXd::Identity(6,6);
			    I.block(3,3,3,3) *= 0.5;
			}

			setQfromq();
			makevG();

			int k;
			for(k=0; k<8; ++k) //Loop on Gauss Point
			{
				double xi, eta, zeta;

				xi = gau(k,0);
				eta = gau(k,1);
				zeta = gau(k,2);

			    makeG(k);
			    makeTheta();
			    makeA();
			    makeB();
			    makeJ0(xi, eta, zeta);   // Computation of H, J0 and J0i
			    makeF();
			    makeC();
			    integrateDamage(k,0);


			    vkappaConv(k) = kappa;
			    vCConv.col(k) = C;
			    vBConv.block(6*k,0,6,24) = B;

			    material->calcW(C,kappa);
			    vWConv(k) = material->getW();
			}
		}
		// Viscoelasticity
		else if (!material->type.compare("viscoelastic"))
		{
			if(vWConv.size() == 0)	vWConv.setZero(8);
			if(CMid.size() == 0)    	CMid.setZero(6);      // Right Cauchy-Green Deformation Tensor at mid-point
			if(vBConv.size() == 0)  	vBConv.setZero(48,24);
			if(vCConv.size() == 0)
            {
                vCConv.setZero(6,8);
                vCConv.block(0,0,3,8) << 1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,
                                         1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,
                                         1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0;
            }
			if(SMid.size() == 0)        SMid.setZero(6);         // Second Piola-Kirchhoff stress tensor at mid-ponit (Vector format)
			if(Gam.size() == 0)         Gam.setZero(6*nInt);
			if(vGamConv.size() == 0)    vGamConv.setZero(6*nInt,8);
			if(GamMid.size() == 0)      GamMid.setZero(6*nInt);
			if(Qv.size() == 0)          Qv.setZero(6*nInt);
			if(QvMid.size() == 0)       QvMid.setZero(6*nInt);
			if(Kiv.size()==0) Kiv.setZero(6*nInt,6*nInt);
			if(CrossKg.size() == 0) CrossKg.setZero(6);
			if(auxGam.size() == 0 and nInt == 2) auxGam.setZero(6*nInt);
			if(auxQv.size() == 0 and nInt == 2) auxQv.setZero(6);
		
			if(I.size() == 0)		I.setZero(6,6);		// Identity fourth-order tensor in Voigt format.
			if(Cross.size() == 0)	Cross.setZero(6,6);	// Consistent tangent matrix term.
		
//			I = MatrixXd::Identity(6,6);

            if(I(0,0) != 1.0)
            {
            	I = MatrixXd::Identity(6,6);
            	I.block(3,3,3,3) *= 0.5;
            }
		
            setQfromq();
            makevG();

            int k;
            for(k=0; k<8; ++k) //Loop on Gauss Point
            {
	    		double xi, eta, zeta;

	    		xi = gau(k,0);
	            eta = gau(k,1);
	            zeta = gau(k,2);

            	makeG(k);
            	makeTheta();
            	makeA();
            	makeB();
            	makeJ0(xi, eta, zeta);   // Computation of H, J0 and J0i
                makeF();
                makeC();
				integrateInternalStrains(k,0);
			
				vGamConv.col(k) = Gam;
				vCConv.col(k) = C;
				vBConv.block(6*k,0,6,24) = B;

				material->calcW(C,Gam);
				vWConv(k) = material->getW();
			}
		}
		// Thermoelasticity
		else if (!material->type.compare("thermoelastic"))
		{
			if(vWConv.size() == 0)	vWConv.setZero(8);
			if(CMid.size() == 0)    	CMid.setZero(6);      // Right Cauchy-Green Deformation Tensor at mid-point
			if(vBConv.size() == 0)  	vBConv.setZero(48,24);
			if(vCConv.size() == 0)
            {
                vCConv.setZero(6,8);
                vCConv.block(0,0,3,8) << 1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,
                                         1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,
                                         1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0;
            }
			if(SMid.size() == 0)        SMid.setZero(6);         // Second Piola-Kirchhoff stress tensor at mid-ponit (Vector format)

			setQfromq();
			makevG();

			int k;		//cout << "rho= " << material->getDensity() << endl;
			for(k=0; k<8; ++k) //Loop on Gauss Point
			{
				double xi, eta, zeta;

				xi = gau(k,0);
				eta = gau(k,1);
				zeta = gau(k,2);

				makeG(k);
				makeTheta();
				makeA();
				makeB();
				makeJ0(xi, eta, zeta);   // Computation of H, J0 and J0i
				makeF();
				makeC();

				vGamConv.col(k) = Gam;
				vCConv.col(k) = C;
				vBConv.block(6*k,0,6,24) = B;

				vThetaConv.setZero(8);
				for (int i=0; i<8;++i) // Loop on shape function
					vThetaConv(k) += calcShape(i,xi,eta,zeta) * q[numDofsInertia + i];

				// vThetaConv(k) = Nt.row(k) * thetaVec;

				material->calcW(C,vThetaConv(k));
				vWConv(k) = material->getW();
			}
		}
	}
	// Consistent integrator in static Analysis
	if (integratorType == 3)
    {
		// Viscoelasticity
		if (!material->type.compare("viscoelastic"))
		{
			if(Gam.size() == 0)   Gam.setZero(6*nInt);
            if(GamMid.size() == 0)  GamMid.setZero(6*nInt);
            if(vCConv.size() == 0)
            {
                vCConv.setZero(6,8);
                vCConv.block(0,0,3,8) << 1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,
                                         1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,
                                         1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0;
            }
            if(vGamConv.size() == 0)  vGamConv.setZero(6*nInt,8);
            if(Qv.size() == 0)      Qv.setZero(6*nInt);
            if(QvMid.size() == 0)   QvMid.setZero(6*nInt);
            if(auxGam.size() == 0 and nInt == 2) auxGam.setZero(6*nInt);

            int k;
            setQfromq();

            for(k=0; k<8; ++k) //Loop on Gauss Point
            {
            	double xi, eta, zeta;

                xi = gau(k,0);
                eta = gau(k,1);
                zeta = gau(k,2);

                makeJ0(xi, eta, zeta);   // Computation of H, J0 and J0i
                makeF();
                makeC();
                integrateInternalStrains(k,0);
                        
                vGamConv.col(k) = Gam;
                vCConv.col(k) = C;
            }
		}
    }
}

// ***

void Brick::setIniDofs(vector<double>&)
{}

// ***

double Brick::calcShape(int &num, double &x, double &y, double &z)
{
	double n;
	if(num==0)
	{
		n = (1.0 - x) * (1.0 - y) * (1.0 - z) / 8.0;
	}
	else if(num==1)
	{
		n = (1.0 + x) * (1.0 - y) * (1.0 - z) / 8.0;
	}
	else if(num==2)
	{
		n = (1.0 + x) * (1.0 + y) * (1.0 - z) / 8.0;
	}
	else if(num==3)
	{
		n = (1.0 - x) * (1.0 + y) * (1.0 - z) / 8.0;
	}
	else if(num==4)
	{
		n = (1.0 - x) * (1.0 - y) * (1.0 + z) / 8.0;
	}
	else if(num==5)
	{
		n = (1.0 + x) * (1.0 - y) * (1.0 + z) / 8.0;
	}
	else if(num==6)
	{
		n = (1.0 + x) * (1.0 + y) * (1.0 + z) / 8.0;
	}
	else if(num==7)
	{
		n = (1.0 - x) * (1.0 + y) * (1.0 + z) / 8.0;
	}

	return n;
}

// ***

void Brick::calcPhi(const vector<double>& q)
{}

// ***

void Brick::calcDPhi(const vector<double>& q)
{}

// ***

void Brick::calcIntegShape()
{
	int i;
	int k;
	
	for (i=0; i<8; ++i)	// Loop on shape functions
	{
		for (k=0; k<8; ++k)	// Loop on Gauss points
		{
        	double xi, eta, zeta;

        	xi = gau(k,0);
        	eta = gau(k,1);
        	zeta = gau(k,2);

			makeJ0(xi, eta, zeta);	// Computation of H, J0 and J0i
			integShape(i) += detJ0 * calcShape(i, xi, eta, zeta);
		}
	}
}

// ***

double Brick::calcElementMass()
{
	// Computes the mass of the element
	int k;
	double mass = 0.0;
	for (k=0; k<8; ++k)	// Loop on Gauss points
	{
    	double xi, eta, zeta;

    	xi = gau(k,0);
    	eta = gau(k,1);
    	zeta = gau(k,2);

		makeJ0(xi, eta, zeta);	// Computation of H, J0 and J0i
		mass += detJ0;	// Volume
	}
	mass *= material->getDensity();
	return mass;
}
// ***

void Brick::calcMSmooth(const vector<double>& q_in, const double& t)
{
	int i;
	int j;
	
	MSmooth.setZero(numDofsInertia, numDofsInertia);

//	if(MSmooth.size()==0)	// Just in case M has not been created in initializeCompRes
//	{
//		MSmooth.resize(numDofsInertia);
//		for(i=0; i<numDofsInertia;++i)	MSmooth[i].resize(numDofsInertia);
//	}
	
	
	if (M.size()==0)  calcM(q,0.0);	// Just in case mass matrix not available

	if (material->getDensity()!=0.0)
	{
//		for(i=0; i < numDofs; i++)
//		{
//			for(j=0; j < numDofs; j++)
//			{
				MSmooth = 1.0 /(material->getDensity()) * M ;
//			}
//		}
	}
	else
	{
//	  	for(i=0; i < numDofs; i++)
//		{
//			for(j=0; j < numDofs; j++)
//			{
				MSmooth = M;
//			}
//		}
	}
	/*
	cout << "MSmooth is " << endl;
	for (i=0; i<numDofsInertia; ++i)
	{
		for (j=0; j<numDofsInertia; ++j)
		{
			cout << MSmooth[i,j) << " ";
		}
		cout << endl;
	}
	*/
	/*
	cout << "Diagonal de MSmooth is " << endl;
	for (i=0; i<numDofsInertia; ++i)
	{
		cout << MSmooth[i,i) << " ";
	}
			cout << endl;
	*/
}

// ***

void Brick::calcFSmooth(const vector<double>& q_in, const vector<double>& dq_in, const double& t)
{
	int k;
	double detF;
	double xi;
	double eta;
	double zeta;
	
	Sigma.setZero(6);
	
	//if (FSmooth.size()==0)		// Just in case M has not been created in initializeCompRes
//	{
	FSmooth.setZero(2,numDofsInertia);
		//FSmooth.resize(2);
		//for (k=0; k<2; ++k)	FSmooth[k].resize(numDofsInertia);
	//}
	//else FSmooth.setZero();
	
	//if (Sigma.size()==0)	Sigma.resize(6);
	if (PSigma.size()==0)	PSigma.setZero(dim);
	
	//q = q_in;
	//setQfromq[);
	
	//makeU();
	
	for (k=0; k<8; ++k)	// Loop on Gauss points
	{
		xi = gau(k,0);
		eta = gau(k,1);
		zeta = gau(k,2);
		makeG(k);
		makeTheta();
		makeA();
		makeB();
		makeJ0(xi, eta, zeta);	// Computation of H, J0 and J0i
		makeF();
		detF = F.determinant(); //F(0,0)*F(1,1)*F(2,2)-F(0,0)*F(1,2)*F(2,1)-F(1,0)*F(0,1)*F(2,2)+F(1,0)*F(0,2)*F(2,1)+F(2,0)*F(0,1)*F(1,2)-F(2,0)*F(0,2)*F(1,1);

		//cout << "F= " << endl << F << endl;
		if (detF!=0.0)
		{	
			InverseVoigtFormat(SMat, S);
			SigmaMat = 1/detF * F * SMat * F.transpose();

			//cout << "Sigma= " << endl << SigmaMat << endl;

			VoigtFormat(SigmaMat, Sigma);

			//Sigma(0) = 1/detF*((F(0,0)*S(0)+F(0,1)*S(3)+F(0,2)*S(4))*F(0,0)+(F(0,0)*S(3)+F(0,1)*S(1)+F(0,2)*S(5))*F(0,1)+(F(0,0)*S(4)+F(0,1)*S(5)+F(0,2)*S(2))*F(0,2));
			//Sigma(3) = 1/detF*((F(0,0)*S(0)+F(0,1)*S(3)+F(0,2)*S(4))*F(1,0)+(F(0,0)*S(3)+F(0,1)*S(1)+F(0,2)*S(5))*F(1,1)+(F(0,0)*S(4)+F(0,1)*S(5)+F(0,2)*S(2))*F(1,2));
			//Sigma(4) = 1/detF*((F(0,0)*S(0)+F(0,1)*S(3)+F(0,2)*S(4))*F(2,0)+(F(0,0)*S(3)+F(0,1)*S(1)+F(0,2)*S(5))*F(2,1)+(F(0,0)*S(4)+F(0,1)*S(5)+F(0,2)*S(2))*F(2,2));
			//Sigma(1) = 1/detF*((F(1,0)*S(0)+F(1,1)*S(3)+F(1,2)*S(4))*F(1,0)+(F(1,0)*S(3)+F(1,1)*S(1)+F(1,2)*S(5))*F(1,1)+(F(1,0)*S(4)+F(1,1)*S(5)+F(1,2)*S(2))*F(1,2));
			//Sigma(5) = 1/detF*((F(1,0)*S(0)+F(1,1)*S(3)+F(1,2)*S(4))*F(2,0)+(F(1,0)*S(3)+F(1,1)*S(1)+F(1,2)*S(5))*F(2,1)+(F(1,0)*S(4)+F(1,1)*S(5)+F(1,2)*S(2))*F(2,2));
			//Sigma(2) = 1/detF*((F(2,0)*S(0)+F(2,1)*S(3)+F(2,2)*S(4))*F(2,0)+(F(2,0)*S(3)+F(2,1)*S(1)+F(2,2)*S(5))*F(2,1)+(F(2,0)*S(4)+F(2,1)*S(5)+F(2,2)*S(2))*F(2,2));
		}
		else Sigma.setZero(6);
		
//		cout << "Na= " << endl << N << endl;
//
		for (int i=0; i<8; ++i)	// Loop on shape function
		{
			N.block(0,3*i,3,3) = calcShape(i, xi, eta, zeta) * MatrixXd::Identity(3,3);
		}

//		cout << "Nd= " << endl << N << endl;

		FSmooth.row(0) += (Sigma.segment<3>(0)).transpose() * N *  detJ0;
		FSmooth.row(1) += (Sigma.segment<3>(3)).transpose() * N *  detJ0;

		//cout << "FSmooth= " << endl << FSmooth << endl;

//		FSmooth(0,0) += detJ0*(1.0-xi)*(1.0-eta)*(1.0-zeta)*Sigma(0)/8.0;
//		FSmooth(0,1) += detJ0*(1.0-xi)*(1.0-eta)*(1.0-zeta)*Sigma(1)/8.0;
//		FSmooth(0,2) += detJ0*(1.0-xi)*(1.0-eta)*(1.0-zeta)*Sigma(2)/8.0;
//		FSmooth(0,3) += detJ0*(1.0+xi)*(1.0-eta)*(1.0-zeta)*Sigma(0)/8.0;
//		FSmooth(0,4) += detJ0*(1.0+xi)*(1.0-eta)*(1.0-zeta)*Sigma(1)/8.0;
//		FSmooth(0,5) += detJ0*(1.0+xi)*(1.0-eta)*(1.0-zeta)*Sigma(2)/8.0;
//		FSmooth(0,6) += detJ0*(1.0+xi)*(1.0+eta)*(1.0-zeta)*Sigma(0)/8.0;
//		FSmooth(0,7) += detJ0*(1.0+xi)*(1.0+eta)*(1.0-zeta)*Sigma(1)/8.0;
//		FSmooth(0,8) += detJ0*(1.0+xi)*(1.0+eta)*(1.0-zeta)*Sigma(2)/8.0;
//		FSmooth(0,9) += detJ0*(1.0-xi)*(1.0+eta)*(1.0-zeta)*Sigma(0)/8.0;
//		FSmooth(0,10) += detJ0*(1.0-xi)*(1.0+eta)*(1.0-zeta)*Sigma(1)/8.0;
//		FSmooth(0,11) += detJ0*(1.0-xi)*(1.0+eta)*(1.0-zeta)*Sigma(2)/8.0;
//		FSmooth(0,12) += detJ0*(1.0-xi)*(1.0-eta)*(1.0+zeta)*Sigma(0)/8.0;
//		FSmooth(0,13) += detJ0*(1.0-xi)*(1.0-eta)*(1.0+zeta)*Sigma(1)/8.0;
//		FSmooth(0,14) += detJ0*(1.0-xi)*(1.0-eta)*(1.0+zeta)*Sigma(2)/8.0;
//		FSmooth(0,15) += detJ0*(1.0+xi)*(1.0-eta)*(1.0+zeta)*Sigma(0)/8.0;
//		FSmooth(0,16) += detJ0*(1.0+xi)*(1.0-eta)*(1.0+zeta)*Sigma(1)/8.0;
//		FSmooth(0,17) += detJ0*(1.0+xi)*(1.0-eta)*(1.0+zeta)*Sigma(2)/8.0;
//		FSmooth(0,18) += detJ0*(1.0+xi)*(1.0+eta)*(1.0+zeta)*Sigma(0)/8.0;
//		FSmooth(0,19) += detJ0*(1.0+xi)*(1.0+eta)*(1.0+zeta)*Sigma(1)/8.0;
//		FSmooth(0,20) += detJ0*(1.0+xi)*(1.0+eta)*(1.0+zeta)*Sigma(2)/8.0;
//		FSmooth(0,21) += detJ0*(1.0-xi)*(1.0+eta)*(1.0+zeta)*Sigma(0)/8.0;
//		FSmooth(0,22) += detJ0*(1.0-xi)*(1.0+eta)*(1.0+zeta)*Sigma(1)/8.0;
//		FSmooth(0,23) += detJ0*(1.0-xi)*(1.0+eta)*(1.0+zeta)*Sigma(2)/8.0;
//
//		FSmooth(1,0) += detJ0*(1.0-xi)*(1.0-eta)*(1.0-zeta)*Sigma(3)/8.0;
//		FSmooth(1,1) += detJ0*(1.0-xi)*(1.0-eta)*(1.0-zeta)*Sigma(4)/8.0;
//		FSmooth(1,2) += detJ0*(1.0-xi)*(1.0-eta)*(1.0-zeta)*Sigma(5)/8.0;
//		FSmooth(1,3) += detJ0*(1.0+xi)*(1.0-eta)*(1.0-zeta)*Sigma(3)/8.0;
//		FSmooth(1,4) += detJ0*(1.0+xi)*(1.0-eta)*(1.0-zeta)*Sigma(4)/8.0;
//		FSmooth(1,5) += detJ0*(1.0+xi)*(1.0-eta)*(1.0-zeta)*Sigma(5)/8.0;
//		FSmooth(1,6) += detJ0*(1.0+xi)*(1.0+eta)*(1.0-zeta)*Sigma(3)/8.0;
//		FSmooth(1,7) += detJ0*(1.0+xi)*(1.0+eta)*(1.0-zeta)*Sigma(4)/8.0;
//		FSmooth(1,8) += detJ0*(1.0+xi)*(1.0+eta)*(1.0-zeta)*Sigma(5)/8.0;
//		FSmooth(1,9) += detJ0*(1.0-xi)*(1.0+eta)*(1.0-zeta)*Sigma(3)/8.0;
//		FSmooth(1,10) += detJ0*(1.0-xi)*(1.0+eta)*(1.0-zeta)*Sigma(4)/8.0;
//		FSmooth(1,11) += detJ0*(1.0-xi)*(1.0+eta)*(1.0-zeta)*Sigma(5)/8.0;
//		FSmooth(1,12) += detJ0*(1.0-xi)*(1.0-eta)*(1.0+zeta)*Sigma(3)/8.0;
//		FSmooth(1,13) += detJ0*(1.0-xi)*(1.0-eta)*(1.0+zeta)*Sigma(4)/8.0;
//		FSmooth(1,14) += detJ0*(1.0-xi)*(1.0-eta)*(1.0+zeta)*Sigma(5)/8.0;
//		FSmooth(1,15) += detJ0*(1.0+xi)*(1.0-eta)*(1.0+zeta)*Sigma(3)/8.0;
//		FSmooth(1,16) += detJ0*(1.0+xi)*(1.0-eta)*(1.0+zeta)*Sigma(4)/8.0;
//		FSmooth(1,17) += detJ0*(1.0+xi)*(1.0-eta)*(1.0+zeta)*Sigma(5)/8.0;
//		FSmooth(1,18) += detJ0*(1.0+xi)*(1.0+eta)*(1.0+zeta)*Sigma(3)/8.0;
//		FSmooth(1,19) += detJ0*(1.0+xi)*(1.0+eta)*(1.0+zeta)*Sigma(4)/8.0;
//		FSmooth(1,20) += detJ0*(1.0+xi)*(1.0+eta)*(1.0+zeta)*Sigma(5)/8.0;
//		FSmooth(1,21) += detJ0*(1.0-xi)*(1.0+eta)*(1.0+zeta)*Sigma(3)/8.0;
//		FSmooth(1,22) += detJ0*(1.0-xi)*(1.0+eta)*(1.0+zeta)*Sigma(4)/8.0;
//		FSmooth(1,23) += detJ0*(1.0-xi)*(1.0+eta)*(1.0+zeta)*Sigma(5)/8.0;

		//printSTLMatrix(FSmooth);
		
		/*
		SigmaMat(0,0) = Sigma(0);
		SigmaMat(1,1) = Sigma(1);
		SigmaMat(2,2) = Sigma(2);
		SigmaMat(0,1) = Sigma(3);
		SigmaMat(0,2) = Sigma(4);
		SigmaMat(1,2) = Sigma(5);
		
		SigmaMat(1,0) = SigmaMat(0,1);
		SigmaMat(2,0) = SigmaMat(0,2);
		SigmaMat(2,1) = SigmaMat(1,2);
		
		SelfAdjointEigenSolver<Matrix3d> eigensolver(SigmaMat);
		PSigma(0) = eigensolver.eigenvalues()(0);
		PSigma(1) = eigensolver.eigenvalues()(1);
		PSigma(2) = eigensolver.eigenvalues()(2);
		sort(PSigma.begin(), PSigma.end());
		reverse(PSigma.begin(), PSigma.end());		
		
		FSmooth(2,0) += detJ0*(1.0-xi)*(1.0-eta)*(1.0-zeta)*PSigma(0)/8.0;
		FSmooth(2,1) += detJ0*(1.0-xi)*(1.0-eta)*(1.0-zeta)*PSigma(1)/8.0;
		FSmooth(2,2) += detJ0*(1.0-xi)*(1.0-eta)*(1.0-zeta)*PSigma(2)/8.0;
		FSmooth(2,3) += detJ0*(1.0+xi)*(1.0-eta)*(1.0-zeta)*PSigma(0)/8.0;
		FSmooth(2,4) += detJ0*(1.0+xi)*(1.0-eta)*(1.0-zeta)*PSigma(1)/8.0;
		FSmooth(2,5) += detJ0*(1.0+xi)*(1.0-eta)*(1.0-zeta)*PSigma(2)/8.0;
		FSmooth(2,6) += detJ0*(1.0+xi)*(1.0+eta)*(1.0-zeta)*PSigma(0)/8.0;
		FSmooth(2,7) += detJ0*(1.0+xi)*(1.0+eta)*(1.0-zeta)*PSigma(1)/8.0;
		FSmooth(2,8) += detJ0*(1.0+xi)*(1.0+eta)*(1.0-zeta)*PSigma(2)/8.0;
		FSmooth(2,9) += detJ0*(1.0-xi)*(1.0+eta)*(1.0-zeta)*PSigma(0)/8.0;
		FSmooth(2,10) += detJ0*(1.0-xi)*(1.0+eta)*(1.0-zeta)*PSigma(1)/8.0;
		FSmooth(2,11) += detJ0*(1.0-xi)*(1.0+eta)*(1.0-zeta)*PSigma(2)/8.0;
		FSmooth(2,12) += detJ0*(1.0-xi)*(1.0-eta)*(1.0+zeta)*PSigma(0)/8.0;
		FSmooth(2,13) += detJ0*(1.0-xi)*(1.0-eta)*(1.0+zeta)*PSigma(1)/8.0;
		FSmooth(2,14) += detJ0*(1.0-xi)*(1.0-eta)*(1.0+zeta)*PSigma(2)/8.0;
		FSmooth(2,15) += detJ0*(1.0+xi)*(1.0-eta)*(1.0+zeta)*PSigma(0)/8.0;
		FSmooth(2,16) += detJ0*(1.0+xi)*(1.0-eta)*(1.0+zeta)*PSigma(1)/8.0;
		FSmooth(2,17) += detJ0*(1.0+xi)*(1.0-eta)*(1.0+zeta)*PSigma(2)/8.0;
		FSmooth(2,18) += detJ0*(1.0+xi)*(1.0+eta)*(1.0+zeta)*PSigma(0)/8.0;
		FSmooth(2,19) += detJ0*(1.0+xi)*(1.0+eta)*(1.0+zeta)*PSigma(1)/8.0;
		FSmooth(2,20) += detJ0*(1.0+xi)*(1.0+eta)*(1.0+zeta)*PSigma(2)/8.0;
		FSmooth(2,21) += detJ0*(1.0-xi)*(1.0+eta)*(1.0+zeta)*PSigma(0)/8.0;
		FSmooth(2,22) += detJ0*(1.0-xi)*(1.0+eta)*(1.0+zeta)*PSigma(1)/8.0;
		FSmooth(2,23) += detJ0*(1.0-xi)*(1.0+eta)*(1.0+zeta)*PSigma(2)/8.0;
		
		VMStress = sqrt( ( (PSigma(0)-PSigma(1))*(PSigma(0)-PSigma(1)) +  (PSigma(1)-PSigma(2))*(PSigma(1)-PSigma(2)) +  (PSigma(2)-PSigma(0))*(PSigma(2)-PSigma(0)) )/2.0 );
		
		FSmooth(3,0) += detJ0*(1.0-xi)*(1.0-eta)*(1.0-zeta)*VMStress/8.0;
		FSmooth(3,1) += detJ0*(1.0-xi)*(1.0-eta)*(1.0-zeta)*VMStress/8.0;
		FSmooth(3,2) += detJ0*(1.0-xi)*(1.0-eta)*(1.0-zeta)*VMStress/8.0;
		FSmooth(3,3) += detJ0*(1.0+xi)*(1.0-eta)*(1.0-zeta)*VMStress/8.0;
		FSmooth(3,4) += detJ0*(1.0+xi)*(1.0-eta)*(1.0-zeta)*VMStress/8.0;
		FSmooth(3,5) += detJ0*(1.0+xi)*(1.0-eta)*(1.0-zeta)*VMStress/8.0;
		FSmooth(3,6) += detJ0*(1.0+xi)*(1.0+eta)*(1.0-zeta)*VMStress/8.0;
		FSmooth(3,7) += detJ0*(1.0+xi)*(1.0+eta)*(1.0-zeta)*VMStress/8.0;
		FSmooth(3,8) += detJ0*(1.0+xi)*(1.0+eta)*(1.0-zeta)*VMStress/8.0;
		FSmooth(3,9) += detJ0*(1.0-xi)*(1.0+eta)*(1.0-zeta)*VMStress/8.0;
		FSmooth(3,10) += detJ0*(1.0-xi)*(1.0+eta)*(1.0-zeta)*VMStress/8.0;
		FSmooth(3,11) += detJ0*(1.0-xi)*(1.0+eta)*(1.0-zeta)*VMStress/8.0;
		FSmooth(3,12) += detJ0*(1.0-xi)*(1.0-eta)*(1.0+zeta)*VMStress/8.0;
		FSmooth(3,13) += detJ0*(1.0-xi)*(1.0-eta)*(1.0+zeta)*VMStress/8.0;
		FSmooth(3,14) += detJ0*(1.0-xi)*(1.0-eta)*(1.0+zeta)*VMStress/8.0;
		FSmooth(3,15) += detJ0*(1.0+xi)*(1.0-eta)*(1.0+zeta)*VMStress/8.0;
		FSmooth(3,16) += detJ0*(1.0+xi)*(1.0-eta)*(1.0+zeta)*VMStress/8.0;
		FSmooth(3,17) += detJ0*(1.0+xi)*(1.0-eta)*(1.0+zeta)*VMStress/8.0;
		FSmooth(3,18) += detJ0*(1.0+xi)*(1.0+eta)*(1.0+zeta)*VMStress/8.0;
		FSmooth(3,19) += detJ0*(1.0+xi)*(1.0+eta)*(1.0+zeta)*VMStress/8.0;
		FSmooth(3,20) += detJ0*(1.0+xi)*(1.0+eta)*(1.0+zeta)*VMStress/8.0;
		FSmooth(3,21) += detJ0*(1.0-xi)*(1.0+eta)*(1.0+zeta)*VMStress/8.0;
		FSmooth(3,22) += detJ0*(1.0-xi)*(1.0+eta)*(1.0+zeta)*VMStress/8.0;
		FSmooth(3,23) += detJ0*(1.0-xi)*(1.0+eta)*(1.0+zeta)*VMStress/8.0;
		*/
	}
	/*
	for (k=0; k<8; ++k)	// Loop on nodes
	{
		// Cauchy stress of the node
		SigmaMat(0,0) = FSmooth[0,dim*k);
		SigmaMat(1,1) = FSmooth[0,dim*k+1);
		SigmaMat(2,2) = FSmooth[0,dim*k+2);
		SigmaMat(0,1) = FSmooth[1,dim*k);
		SigmaMat(0,2) = FSmooth[1,dim*k+1);
		SigmaMat(1,2) = FSmooth[1,dim*k+2);
		SigmaMat(1,0) = SigmaMat(0,1);
		SigmaMat(2,0) = SigmaMat(0,2);
		SigmaMat(2,1) = SigmaMat(1,2);		
		
		// Ordered eigenvalues (largest first)
		SelfAdjointEigenSolver<Matrix3d> eigensolver(SigmaMat);
		PSigma(0) = eigensolver.eigenvalues()(0);
		PSigma(1) = eigensolver.eigenvalues()(1);
		PSigma(2) = eigensolver.eigenvalues()(2);
		sort(PSigma.begin(), PSigma.end());
		reverse(PSigma.begin(), PSigma.end());		
		FSmooth[2,dim*k) = PSigma(0);
		FSmooth[2,dim*k+1) = PSigma(1);
		FSmooth[2,dim*k+2) = PSigma(2);
		
		// Von Mises
		FSmooth[3,dim*k) = sqrt( ( (PSigma(0)-PSigma(1))*(PSigma(0)-PSigma(1)) +  (PSigma(1)-PSigma(2))*(PSigma(1)-PSigma(2)) +  (PSigma(2)-PSigma(0))*(PSigma(2)-PSigma(0)) )/2.0 );
		// Other stress measurements, TO BE DONE
		FSmooth[3,dim*k+1) = FSmooth[3,dim*k);
		FSmooth[3,dim*k+2) = FSmooth[3,dim*k);		
	}
*/
	/*
	int i;
	int j;
	cout << "FSmooth is " << endl;
	for (i=0; i<2; ++i)
	{
		for (j=0; j<numDofsInertia; ++j)
		{
			cout << FSmooth[i,j) << " ";
		}
		cout << endl;
	}
	*/
}

int Brick::checkState()
{
return 0;
}
